CPD Results

The following document contains the results of PMD's CPD 6.4.0.

Duplications

File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 2311
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 2271
            final KnownBiasTurntableGyroscopeCalibratorListener listener) {
        this(convertPosition(position), turntableRotationRate, timeInterval,
                measurements, commonAxisUsed, estimateGDependentCrossBiases,
                bias, initialMg, initialGg, accelerometerBias,
                accelerometerMa, listener);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasX() {
        return mAccelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final double accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasY() {
        return mAccelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final double accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasZ() {
        return mAccelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final double accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(mAccelerometerBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(mAccelerometerBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(mAccelerometerBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final double accelerometerBiasX,
            final double accelerometerBiasY,
            final double accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = accelerometerBiasX;
        mAccelerometerBiasY = accelerometerBiasY;
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public double[] getAccelerometerBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = mAccelerometerBiasX;
        result[1] = mAccelerometerBiasY;
        result[2] = mAccelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void setAccelerometerBias(final double[] accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias[0];
        mAccelerometerBiasY = accelerometerBias[1];
        mAccelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerBiasX);
        result.setElementAtIndex(1, mAccelerometerBiasY);
        result.setElementAtIndex(2, mAccelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setAccelerometerBias(final Matrix accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS
                || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        mAccelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        mAccelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    public double getAccelerometerSx() {
        return mAccelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSx(final double accelerometerSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    public double getAccelerometerSy() {
        return mAccelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSy(final double accelerometerSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    public double getAccelerometerSz() {
        return mAccelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSz(final double accelerometerSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    public double getAccelerometerMxy() {
        return mAccelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxy(final double accelerometerMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    public double getAccelerometerMxz() {
        return mAccelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxz(final double accelerometerMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    public double getAccelerometerMyx() {
        return mAccelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyx(final double accelerometerMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    public double getAccelerometerMyz() {
        return mAccelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyz(final double accelerometerMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    public double getAccelerometerMzx() {
        return mAccelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzx(final double accelerometerMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    public double getAccelerometerMzy() {
        return mAccelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzy(final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
        mAccelerometerSy = accelerometerSy;
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
        mAccelerometerMxz = accelerometerMxz;
        mAccelerometerMyx = accelerometerMyx;
        mAccelerometerMyz = accelerometerMyz;
        mAccelerometerMzx = accelerometerMzx;
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy,
                accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy,
                accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx,
                accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerSx);
        result.setElementAtIndex(1, mAccelerometerMyx);
        result.setElementAtIndex(2, mAccelerometerMzx);

        result.setElementAtIndex(3, mAccelerometerMxy);
        result.setElementAtIndex(4, mAccelerometerSy);
        result.setElementAtIndex(5, mAccelerometerMzy);

        result.setElementAtIndex(6, mAccelerometerMxz);
        result.setElementAtIndex(7, mAccelerometerMyz);
        result.setElementAtIndex(8, mAccelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setAccelerometerMa(final Matrix accelerometerMa)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerSx = accelerometerMa.getElementAtIndex(0);
        mAccelerometerMyx = accelerometerMa.getElementAtIndex(1);
        mAccelerometerMzx = accelerometerMa.getElementAtIndex(2);

        mAccelerometerMxy = accelerometerMa.getElementAtIndex(3);
        mAccelerometerSy = accelerometerMa.getElementAtIndex(4);
        mAccelerometerMzy = accelerometerMa.getElementAtIndex(5);

        mAccelerometerMxz = accelerometerMa.getElementAtIndex(6);
        mAccelerometerMyz = accelerometerMa.getElementAtIndex(7);
        mAccelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets known x-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return known x-coordinate of gyroscope bias.
     */
    public double getBiasX() {
        return mBiasX;
    }

    /**
     * Sets known x-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasX known x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final double biasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
    }

    /**
     * Gets known y-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return known y-coordinate of gyroscope bias.
     */
    public double getBiasY() {
        return mBiasY;
    }

    /**
     * Sets known y-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasY known y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final double biasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = biasY;
    }

    /**
     * Gets known z-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return known z-coordinate of gyroscope bias.
     */
    public double getBiasZ() {
        return mBiasZ;
    }

    /**
     * Sets known z-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasZ known z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final double biasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = biasZ;
    }

    /**
     * Gets known x-coordinate of gyroscope bias.
     *
     * @return known x-coordinate of gyroscope bias.
     */
    public AngularSpeed getBiasAngularSpeedX() {
        return new AngularSpeed(mBiasX,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known x-coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(mBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known x-coordinate of gyroscope bias.
     *
     * @param biasX known x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final AngularSpeed biasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = convertAngularSpeed(biasX);
    }

    /**
     * Gets known y-coordinate of gyroscope bias.
     *
     * @return known y-coordinate of gyroscope bias.
     */
    public AngularSpeed getBiasAngularSpeedY() {
        return new AngularSpeed(mBiasY,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known y-coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(mBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param biasY known y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final AngularSpeed biasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = convertAngularSpeed(biasY);
    }

    /**
     * Gets known z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return known z-coordinate of gyroscope bias.
     */
    public AngularSpeed getBiasAngularSpeedZ() {
        return new AngularSpeed(mBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known z-coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(mBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known z-coordinate of gyroscope bias.
     *
     * @param biasZ known z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final AngularSpeed biasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Sets known bias coordinates of gyroscope expressed in radians
     * per second (rad/s).
     *
     * @param biasX known x-coordinate of gyroscope bias.
     * @param biasY known y-coordinate of gyroscope bias.
     * @param biasZ known z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(
            final double biasX, final double biasY,
            final double biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
        mBiasY = biasY;
        mBiasZ = biasZ;
    }

    /**
     * Sets known bias coordinates of gyroscope.
     *
     * @param biasX known x-coordinate of gyroscope bias.
     * @param biasY known y-coordinate of gyroscope bias.
     * @param biasZ known z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(
            final AngularSpeed biasX,
            final AngularSpeed biasY,
            final AngularSpeed biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = convertAngularSpeed(biasX);
        mBiasY = convertAngularSpeed(biasY);
        mBiasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy,
            final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of gyroscope bias.
     */
    public double[] getBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mBiasX;
        result[1] = mBiasY;
        result[2] = mBiasZ;
    }

    /**
     * Sets known gyroscope bias  as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param bias known bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setBias(final double[] bias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mBiasX = bias[0];
        mBiasY = bias[1];
        mBiasZ = bias[2];
    }

    /**
     * Gets known gyroscope bias as a column matrix.
     *
     * @return known gyroscope bias as a column matrix.
     */
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known gyroscope bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mBiasX);
        result.setElementAtIndex(1, mBiasY);
        result.setElementAtIndex(2, mBiasZ);
    }

    /**
     * Sets known gyroscope bias as an array.
     *
     * @param bias known gyroscope bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setBias(final Matrix bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS
                || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mBiasX = bias.getElementAtIndex(0);
        mBiasY = bias.getElementAtIndex(1);
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @return constant rotation rate of turntable.
     */
    public double getTurntableRotationRate() {
        return mTurntableRotationRate;
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final double turntableRotationRate) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (turntableRotationRate <= 0.0) {
            throw new IllegalArgumentException();
        }

        mTurntableRotationRate = turntableRotationRate;
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @return constant rotation rate of turntable.
     */
    public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
        return new AngularSpeed(mTurntableRotationRate,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @param result instance where result will be stored.
     */
    public void getTurntableRotationRateAsAngularSpeed(
            final AngularSpeed result) {
        result.setValue(mTurntableRotationRate);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final AngularSpeed turntableRotationRate)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setTurntableRotationRate(
                convertAngularSpeed(turntableRotationRate));
    }

    /**
     * Gets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @return time interval between measurements.
     */
    public double getTimeInterval() {
        return mTimeInterval;
    }

    /**
     * Sets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTimeInterval(final double timeInterval)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (timeInterval <= 0.0) {
            throw new IllegalArgumentException();
        }
        mTimeInterval = timeInterval;
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @return time interval between measurements.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(mTimeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @param result instance where result will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(mTimeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between measurements being captured.
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTimeInterval(final Time timeInterval)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setTimeInterval(convertTime(timeInterval));
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 1101
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 1103
            final EasyGyroscopeCalibratorListener listener) {
        this(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa);
        mListener = listener;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasX() {
        return mAccelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final double accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasY() {
        return mAccelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final double accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasZ() {
        return mAccelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final double accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(mAccelerometerBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(mAccelerometerBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(mAccelerometerBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final double accelerometerBiasX,
            final double accelerometerBiasY,
            final double accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = accelerometerBiasX;
        mAccelerometerBiasY = accelerometerBiasY;
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public double[] getAccelerometerBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = mAccelerometerBiasX;
        result[1] = mAccelerometerBiasY;
        result[2] = mAccelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void setAccelerometerBias(final double[] accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias[0];
        mAccelerometerBiasY = accelerometerBias[1];
        mAccelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerBiasX);
        result.setElementAtIndex(1, mAccelerometerBiasY);
        result.setElementAtIndex(2, mAccelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setAccelerometerBias(final Matrix accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS
                || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        mAccelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        mAccelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    public double getAccelerometerSx() {
        return mAccelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSx(final double accelerometerSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    public double getAccelerometerSy() {
        return mAccelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSy(final double accelerometerSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    public double getAccelerometerSz() {
        return mAccelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSz(final double accelerometerSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    public double getAccelerometerMxy() {
        return mAccelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxy(final double accelerometerMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    public double getAccelerometerMxz() {
        return mAccelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxz(final double accelerometerMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    public double getAccelerometerMyx() {
        return mAccelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyx(final double accelerometerMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    public double getAccelerometerMyz() {
        return mAccelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyz(final double accelerometerMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    public double getAccelerometerMzx() {
        return mAccelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzx(final double accelerometerMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    public double getAccelerometerMzy() {
        return mAccelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzy(final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
        mAccelerometerSy = accelerometerSy;
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
        mAccelerometerMxz = accelerometerMxz;
        mAccelerometerMyx = accelerometerMyx;
        mAccelerometerMyz = accelerometerMyz;
        mAccelerometerMzx = accelerometerMzx;
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy,
                accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy,
                accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx,
                accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerSx);
        result.setElementAtIndex(1, mAccelerometerMyx);
        result.setElementAtIndex(2, mAccelerometerMzx);

        result.setElementAtIndex(3, mAccelerometerMxy);
        result.setElementAtIndex(4, mAccelerometerSy);
        result.setElementAtIndex(5, mAccelerometerMzy);

        result.setElementAtIndex(6, mAccelerometerMxz);
        result.setElementAtIndex(7, mAccelerometerMyz);
        result.setElementAtIndex(8, mAccelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setAccelerometerMa(final Matrix accelerometerMa)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerSx = accelerometerMa.getElementAtIndex(0);
        mAccelerometerMyx = accelerometerMa.getElementAtIndex(1);
        mAccelerometerMzx = accelerometerMa.getElementAtIndex(2);

        mAccelerometerMxy = accelerometerMa.getElementAtIndex(3);
        mAccelerometerSy = accelerometerMa.getElementAtIndex(4);
        mAccelerometerMzy = accelerometerMa.getElementAtIndex(5);

        mAccelerometerMxz = accelerometerMa.getElementAtIndex(6);
        mAccelerometerMyz = accelerometerMa.getElementAtIndex(7);
        mAccelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias ot be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(mInitialBiasX,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(mInitialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final AngularSpeed initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(mInitialBiasY,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(mInitialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final AngularSpeed initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(mInitialBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(mInitialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final AngularSpeed initialBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution
     * expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final double initialBiasX, final double initialBiasY,
            final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
        mInitialBiasY = initialBiasY;
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final AngularSpeed initialBiasX,
            final AngularSpeed initialBiasY,
            final AngularSpeed initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAngularSpeed(initialBiasX);
        mInitialBiasY = convertAngularSpeed(initialBiasY);
        mInitialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy,
            final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of initial gyroscope bias.
     */
    public double[] getInitialBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mInitialBiasX;
        result[1] = mInitialBiasY;
        result[2] = mInitialBiasZ;
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mInitialBiasX = initialBias[0];
        mInitialBiasY = initialBias[1];
        mInitialBiasZ = initialBias[2];
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialBiasX);
        result.setElementAtIndex(1, mInitialBiasY);
        result.setElementAtIndex(2, mInitialBiasZ);
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * a column matrix.
     *
     * @param initialBias initial gyroscope bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS
                || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mInitialBiasX = initialBias.getElementAtIndex(0);
        mInitialBiasY = initialBias.getElementAtIndex(1);
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
        return mSequences;
    }

    /**
     * Sets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @param sequences collection of sequences of timestamped body
     *                  kinematics measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setSequences(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mSequences = sequences;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Indicates whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @return true if G-dependent cross biases will be estimated,
     * false otherwise.
     */
    public boolean isGDependentCrossBiasesEstimated() {
        return mEstimateGDependentCrossBiases;
    }

    /**
     * Specifies whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setGDependentCrossBiasesEstimated(
            final boolean estimateGDependentCrossBiases)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mEstimateGDependentCrossBiases = estimateGDependentCrossBiases;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public EasyGyroscopeCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1079
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1081
            final KnownBiasEasyGyroscopeCalibratorListener listener) {
        this(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, bias, initialMg,
                initialGg, accelerometerBias, accelerometerMa);
        mListener = listener;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasX() {
        return mAccelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final double accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasY() {
        return mAccelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final double accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasZ() {
        return mAccelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final double accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(mAccelerometerBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(mAccelerometerBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(mAccelerometerBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final double accelerometerBiasX,
            final double accelerometerBiasY,
            final double accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = accelerometerBiasX;
        mAccelerometerBiasY = accelerometerBiasY;
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public double[] getAccelerometerBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = mAccelerometerBiasX;
        result[1] = mAccelerometerBiasY;
        result[2] = mAccelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void setAccelerometerBias(final double[] accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias[0];
        mAccelerometerBiasY = accelerometerBias[1];
        mAccelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerBiasX);
        result.setElementAtIndex(1, mAccelerometerBiasY);
        result.setElementAtIndex(2, mAccelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setAccelerometerBias(final Matrix accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS
                || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        mAccelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        mAccelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    public double getAccelerometerSx() {
        return mAccelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSx(final double accelerometerSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    public double getAccelerometerSy() {
        return mAccelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSy(final double accelerometerSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    public double getAccelerometerSz() {
        return mAccelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSz(final double accelerometerSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    public double getAccelerometerMxy() {
        return mAccelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxy(final double accelerometerMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    public double getAccelerometerMxz() {
        return mAccelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxz(final double accelerometerMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    public double getAccelerometerMyx() {
        return mAccelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyx(final double accelerometerMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    public double getAccelerometerMyz() {
        return mAccelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyz(final double accelerometerMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    public double getAccelerometerMzx() {
        return mAccelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzx(final double accelerometerMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    public double getAccelerometerMzy() {
        return mAccelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzy(final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
        mAccelerometerSy = accelerometerSy;
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
        mAccelerometerMxz = accelerometerMxz;
        mAccelerometerMyx = accelerometerMyx;
        mAccelerometerMyz = accelerometerMyz;
        mAccelerometerMzx = accelerometerMzx;
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy,
                accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy,
                accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx,
                accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerSx);
        result.setElementAtIndex(1, mAccelerometerMyx);
        result.setElementAtIndex(2, mAccelerometerMzx);

        result.setElementAtIndex(3, mAccelerometerMxy);
        result.setElementAtIndex(4, mAccelerometerSy);
        result.setElementAtIndex(5, mAccelerometerMzy);

        result.setElementAtIndex(6, mAccelerometerMxz);
        result.setElementAtIndex(7, mAccelerometerMyz);
        result.setElementAtIndex(8, mAccelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setAccelerometerMa(final Matrix accelerometerMa)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerSx = accelerometerMa.getElementAtIndex(0);
        mAccelerometerMyx = accelerometerMa.getElementAtIndex(1);
        mAccelerometerMzx = accelerometerMa.getElementAtIndex(2);

        mAccelerometerMxy = accelerometerMa.getElementAtIndex(3);
        mAccelerometerSy = accelerometerMa.getElementAtIndex(4);
        mAccelerometerMzy = accelerometerMa.getElementAtIndex(5);

        mAccelerometerMxz = accelerometerMa.getElementAtIndex(6);
        mAccelerometerMyz = accelerometerMa.getElementAtIndex(7);
        mAccelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return x-coordinate of gyroscope known bias.
     */
    public double getBiasX() {
        return mBiasX;
    }

    /**
     * Sets x-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final double biasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return y-coordinate of gyroscope known bias.
     */
    public double getBiasY() {
        return mBiasY;
    }

    /**
     * Sets y-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasY y-coordinate of gyroscope known  bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final double biasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = biasY;
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return z-coordinate of gyroscope known bias.
     */
    public double getBiasZ() {
        return mBiasZ;
    }

    /**
     * Sets z-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasZ z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final double biasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = biasZ;
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     *
     * @return x-coordinate of gyroscope known bias.
     */
    public AngularSpeed getBiasAngularSpeedX() {
        return new AngularSpeed(mBiasX,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(mBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets x-coordinate of gyroscope known bias.
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final AngularSpeed biasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = convertAngularSpeed(biasX);
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     *
     * @return y-coordinate of gyroscope known bias.
     */
    public AngularSpeed getBiasAngularSpeedY() {
        return new AngularSpeed(mBiasY,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(mBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets y-coordinate of gyroscope known bias.
     *
     * @param biasY y-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final AngularSpeed biasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = convertAngularSpeed(biasY);
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     *
     * @return initial z-coordinate of gyroscope known bias.
     */
    public AngularSpeed getBiasAngularSpeedZ() {
        return new AngularSpeed(mBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(mBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets z-coordinate of gyroscope known bias.
     *
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final AngularSpeed biasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Sets known bias coordinates of gyroscope expressed in
     * radians per second (rad/s).
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @param biasY y-coordinate of gyroscope known bias.
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(
            final double biasX, final double biasY,
            final double biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
        mBiasY = biasY;
        mBiasZ = biasZ;
    }

    /**
     * Sets known bias coordinates of gyroscope.
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @param biasY y-coordinate of gyroscope known bias.
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(
            final AngularSpeed biasX,
            final AngularSpeed biasY,
            final AngularSpeed biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = convertAngularSpeed(biasX);
        mBiasY = convertAngularSpeed(biasY);
        mBiasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy,
            final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets gyroscope known bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of gyroscope known bias.
     */
    public double[] getBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets gyroscope known bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mBiasX;
        result[1] = mBiasY;
        result[2] = mBiasZ;
    }

    /**
     * Sets gyroscope known bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param bias known bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setBias(final double[] bias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mBiasX = bias[0];
        mBiasY = bias[1];
        mBiasZ = bias[2];
    }

    /**
     * Gets gyroscope known bias as a column matrix.
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets gyroscope known bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mBiasX);
        result.setElementAtIndex(1, mBiasY);
        result.setElementAtIndex(2, mBiasZ);
    }

    /**
     * Sets gyroscope known bias as a column matrix.
     *
     * @param initialBias gyroscope known bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setBias(final Matrix initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS
                || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mBiasX = initialBias.getElementAtIndex(0);
        mBiasY = initialBias.getElementAtIndex(1);
        mBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
        return mSequences;
    }

    /**
     * Sets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @param sequences collection of sequences of timestamped body
     *                  kinematics measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setSequences(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mSequences = sequences;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Indicates whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @return true if G-dependent cross biases will be estimated,
     * false otherwise.
     */
    public boolean isGDependentCrossBiasesEstimated() {
        return mEstimateGDependentCrossBiases;
    }

    /**
     * Specifies whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setGDependentCrossBiasesEstimated(
            final boolean estimateGDependentCrossBiases)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mEstimateGDependentCrossBiases = estimateGDependentCrossBiases;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public KnownBiasEasyGyroscopeCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 1106
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 1108
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 2314
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasX() {
        return mAccelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final double accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasY() {
        return mAccelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final double accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasZ() {
        return mAccelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final double accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(mAccelerometerBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(mAccelerometerBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(mAccelerometerBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final double accelerometerBiasX,
            final double accelerometerBiasY,
            final double accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = accelerometerBiasX;
        mAccelerometerBiasY = accelerometerBiasY;
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public double[] getAccelerometerBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = mAccelerometerBiasX;
        result[1] = mAccelerometerBiasY;
        result[2] = mAccelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void setAccelerometerBias(final double[] accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias[0];
        mAccelerometerBiasY = accelerometerBias[1];
        mAccelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerBiasX);
        result.setElementAtIndex(1, mAccelerometerBiasY);
        result.setElementAtIndex(2, mAccelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setAccelerometerBias(final Matrix accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS
                || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        mAccelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        mAccelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    public double getAccelerometerSx() {
        return mAccelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSx(final double accelerometerSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    public double getAccelerometerSy() {
        return mAccelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSy(final double accelerometerSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    public double getAccelerometerSz() {
        return mAccelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSz(final double accelerometerSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    public double getAccelerometerMxy() {
        return mAccelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxy(final double accelerometerMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    public double getAccelerometerMxz() {
        return mAccelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxz(final double accelerometerMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    public double getAccelerometerMyx() {
        return mAccelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyx(final double accelerometerMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    public double getAccelerometerMyz() {
        return mAccelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyz(final double accelerometerMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    public double getAccelerometerMzx() {
        return mAccelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzx(final double accelerometerMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    public double getAccelerometerMzy() {
        return mAccelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzy(final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
        mAccelerometerSy = accelerometerSy;
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
        mAccelerometerMxz = accelerometerMxz;
        mAccelerometerMyx = accelerometerMyx;
        mAccelerometerMyz = accelerometerMyz;
        mAccelerometerMzx = accelerometerMzx;
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy,
                accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy,
                accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx,
                accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerSx);
        result.setElementAtIndex(1, mAccelerometerMyx);
        result.setElementAtIndex(2, mAccelerometerMzx);

        result.setElementAtIndex(3, mAccelerometerMxy);
        result.setElementAtIndex(4, mAccelerometerSy);
        result.setElementAtIndex(5, mAccelerometerMzy);

        result.setElementAtIndex(6, mAccelerometerMxz);
        result.setElementAtIndex(7, mAccelerometerMyz);
        result.setElementAtIndex(8, mAccelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setAccelerometerMa(final Matrix accelerometerMa)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerSx = accelerometerMa.getElementAtIndex(0);
        mAccelerometerMyx = accelerometerMa.getElementAtIndex(1);
        mAccelerometerMzx = accelerometerMa.getElementAtIndex(2);

        mAccelerometerMxy = accelerometerMa.getElementAtIndex(3);
        mAccelerometerSy = accelerometerMa.getElementAtIndex(4);
        mAccelerometerMzy = accelerometerMa.getElementAtIndex(5);

        mAccelerometerMxz = accelerometerMa.getElementAtIndex(6);
        mAccelerometerMyz = accelerometerMa.getElementAtIndex(7);
        mAccelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias ot be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(mInitialBiasX,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(mInitialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final AngularSpeed initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(mInitialBiasY,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(mInitialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final AngularSpeed initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(mInitialBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(mInitialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final AngularSpeed initialBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution
     * expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final double initialBiasX, final double initialBiasY,
            final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
        mInitialBiasY = initialBiasY;
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final AngularSpeed initialBiasX,
            final AngularSpeed initialBiasY,
            final AngularSpeed initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAngularSpeed(initialBiasX);
        mInitialBiasY = convertAngularSpeed(initialBiasY);
        mInitialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy,
            final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of initial gyroscope bias.
     */
    public double[] getInitialBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mInitialBiasX;
        result[1] = mInitialBiasY;
        result[2] = mInitialBiasZ;
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mInitialBiasX = initialBias[0];
        mInitialBiasY = initialBias[1];
        mInitialBiasZ = initialBias[2];
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialBiasX);
        result.setElementAtIndex(1, mInitialBiasY);
        result.setElementAtIndex(2, mInitialBiasZ);
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * a column matrix.
     *
     * @param initialBias initial gyroscope bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS
                || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mInitialBiasX = initialBias.getElementAtIndex(0);
        mInitialBiasY = initialBias.getElementAtIndex(1);
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1084
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 2316
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1086
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 2276
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasX() {
        return mAccelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final double accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasY() {
        return mAccelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final double accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasZ() {
        return mAccelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final double accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(mAccelerometerBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(mAccelerometerBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(mAccelerometerBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final double accelerometerBiasX,
            final double accelerometerBiasY,
            final double accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = accelerometerBiasX;
        mAccelerometerBiasY = accelerometerBiasY;
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public double[] getAccelerometerBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = mAccelerometerBiasX;
        result[1] = mAccelerometerBiasY;
        result[2] = mAccelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void setAccelerometerBias(final double[] accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias[0];
        mAccelerometerBiasY = accelerometerBias[1];
        mAccelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerBiasX);
        result.setElementAtIndex(1, mAccelerometerBiasY);
        result.setElementAtIndex(2, mAccelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setAccelerometerBias(final Matrix accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS
                || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        mAccelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        mAccelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    public double getAccelerometerSx() {
        return mAccelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSx(final double accelerometerSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    public double getAccelerometerSy() {
        return mAccelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSy(final double accelerometerSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    public double getAccelerometerSz() {
        return mAccelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSz(final double accelerometerSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    public double getAccelerometerMxy() {
        return mAccelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxy(final double accelerometerMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    public double getAccelerometerMxz() {
        return mAccelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxz(final double accelerometerMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    public double getAccelerometerMyx() {
        return mAccelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyx(final double accelerometerMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    public double getAccelerometerMyz() {
        return mAccelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyz(final double accelerometerMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    public double getAccelerometerMzx() {
        return mAccelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzx(final double accelerometerMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    public double getAccelerometerMzy() {
        return mAccelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzy(final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
        mAccelerometerSy = accelerometerSy;
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
        mAccelerometerMxz = accelerometerMxz;
        mAccelerometerMyx = accelerometerMyx;
        mAccelerometerMyz = accelerometerMyz;
        mAccelerometerMzx = accelerometerMzx;
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy,
                accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy,
                accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx,
                accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerSx);
        result.setElementAtIndex(1, mAccelerometerMyx);
        result.setElementAtIndex(2, mAccelerometerMzx);

        result.setElementAtIndex(3, mAccelerometerMxy);
        result.setElementAtIndex(4, mAccelerometerSy);
        result.setElementAtIndex(5, mAccelerometerMzy);

        result.setElementAtIndex(6, mAccelerometerMxz);
        result.setElementAtIndex(7, mAccelerometerMyz);
        result.setElementAtIndex(8, mAccelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setAccelerometerMa(final Matrix accelerometerMa)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerSx = accelerometerMa.getElementAtIndex(0);
        mAccelerometerMyx = accelerometerMa.getElementAtIndex(1);
        mAccelerometerMzx = accelerometerMa.getElementAtIndex(2);

        mAccelerometerMxy = accelerometerMa.getElementAtIndex(3);
        mAccelerometerSy = accelerometerMa.getElementAtIndex(4);
        mAccelerometerMzy = accelerometerMa.getElementAtIndex(5);

        mAccelerometerMxz = accelerometerMa.getElementAtIndex(6);
        mAccelerometerMyz = accelerometerMa.getElementAtIndex(7);
        mAccelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return x-coordinate of gyroscope known bias.
     */
    public double getBiasX() {
        return mBiasX;
    }

    /**
     * Sets x-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final double biasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return y-coordinate of gyroscope known bias.
     */
    public double getBiasY() {
        return mBiasY;
    }

    /**
     * Sets y-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasY y-coordinate of gyroscope known  bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final double biasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = biasY;
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return z-coordinate of gyroscope known bias.
     */
    public double getBiasZ() {
        return mBiasZ;
    }

    /**
     * Sets z-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasZ z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final double biasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = biasZ;
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     *
     * @return x-coordinate of gyroscope known bias.
     */
    public AngularSpeed getBiasAngularSpeedX() {
        return new AngularSpeed(mBiasX,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(mBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets x-coordinate of gyroscope known bias.
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final AngularSpeed biasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = convertAngularSpeed(biasX);
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     *
     * @return y-coordinate of gyroscope known bias.
     */
    public AngularSpeed getBiasAngularSpeedY() {
        return new AngularSpeed(mBiasY,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(mBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets y-coordinate of gyroscope known bias.
     *
     * @param biasY y-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final AngularSpeed biasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = convertAngularSpeed(biasY);
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     *
     * @return initial z-coordinate of gyroscope known bias.
     */
    public AngularSpeed getBiasAngularSpeedZ() {
        return new AngularSpeed(mBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(mBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets z-coordinate of gyroscope known bias.
     *
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final AngularSpeed biasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Sets known bias coordinates of gyroscope expressed in
     * radians per second (rad/s).
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @param biasY y-coordinate of gyroscope known bias.
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(
            final double biasX, final double biasY,
            final double biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
        mBiasY = biasY;
        mBiasZ = biasZ;
    }

    /**
     * Sets known bias coordinates of gyroscope.
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @param biasY y-coordinate of gyroscope known bias.
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(
            final AngularSpeed biasX,
            final AngularSpeed biasY,
            final AngularSpeed biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = convertAngularSpeed(biasX);
        mBiasY = convertAngularSpeed(biasY);
        mBiasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy,
            final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets gyroscope known bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of gyroscope known bias.
     */
    public double[] getBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets gyroscope known bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mBiasX;
        result[1] = mBiasY;
        result[2] = mBiasZ;
    }

    /**
     * Sets gyroscope known bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param bias known bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setBias(final double[] bias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mBiasX = bias[0];
        mBiasY = bias[1];
        mBiasZ = bias[2];
    }

    /**
     * Gets gyroscope known bias as a column matrix.
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets gyroscope known bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mBiasX);
        result.setElementAtIndex(1, mBiasY);
        result.setElementAtIndex(2, mBiasZ);
    }

    /**
     * Sets gyroscope known bias as a column matrix.
     *
     * @param initialBias gyroscope known bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setBias(final Matrix initialBias) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 2309
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 2354
            final RobustTurntableGyroscopeCalibratorListener listener) {
        this(convertPosition(position), turntableRotationRate, timeInterval,
                measurements, commonAxisUsed, estimateGDependentCrossBiases,
                initialBias, initialMg, initialGg, accelerometerBias,
                accelerometerMa, listener);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasX() {
        return mAccelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final double accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasY() {
        return mAccelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final double accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasZ() {
        return mAccelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final double accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(mAccelerometerBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(mAccelerometerBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(mAccelerometerBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final double accelerometerBiasX,
            final double accelerometerBiasY,
            final double accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = accelerometerBiasX;
        mAccelerometerBiasY = accelerometerBiasY;
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public double[] getAccelerometerBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = mAccelerometerBiasX;
        result[1] = mAccelerometerBiasY;
        result[2] = mAccelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void setAccelerometerBias(final double[] accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias[0];
        mAccelerometerBiasY = accelerometerBias[1];
        mAccelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerBiasX);
        result.setElementAtIndex(1, mAccelerometerBiasY);
        result.setElementAtIndex(2, mAccelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setAccelerometerBias(final Matrix accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS
                || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        mAccelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        mAccelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    public double getAccelerometerSx() {
        return mAccelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSx(final double accelerometerSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    public double getAccelerometerSy() {
        return mAccelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSy(final double accelerometerSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    public double getAccelerometerSz() {
        return mAccelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSz(final double accelerometerSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    public double getAccelerometerMxy() {
        return mAccelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxy(final double accelerometerMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    public double getAccelerometerMxz() {
        return mAccelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxz(final double accelerometerMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    public double getAccelerometerMyx() {
        return mAccelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyx(final double accelerometerMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    public double getAccelerometerMyz() {
        return mAccelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyz(final double accelerometerMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    public double getAccelerometerMzx() {
        return mAccelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzx(final double accelerometerMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    public double getAccelerometerMzy() {
        return mAccelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzy(final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
        mAccelerometerSy = accelerometerSy;
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
        mAccelerometerMxz = accelerometerMxz;
        mAccelerometerMyx = accelerometerMyx;
        mAccelerometerMyz = accelerometerMyz;
        mAccelerometerMzx = accelerometerMzx;
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy,
                accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy,
                accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx,
                accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerSx);
        result.setElementAtIndex(1, mAccelerometerMyx);
        result.setElementAtIndex(2, mAccelerometerMzx);

        result.setElementAtIndex(3, mAccelerometerMxy);
        result.setElementAtIndex(4, mAccelerometerSy);
        result.setElementAtIndex(5, mAccelerometerMzy);

        result.setElementAtIndex(6, mAccelerometerMxz);
        result.setElementAtIndex(7, mAccelerometerMyz);
        result.setElementAtIndex(8, mAccelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setAccelerometerMa(final Matrix accelerometerMa)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerSx = accelerometerMa.getElementAtIndex(0);
        mAccelerometerMyx = accelerometerMa.getElementAtIndex(1);
        mAccelerometerMzx = accelerometerMa.getElementAtIndex(2);

        mAccelerometerMxy = accelerometerMa.getElementAtIndex(3);
        mAccelerometerSy = accelerometerMa.getElementAtIndex(4);
        mAccelerometerMzy = accelerometerMa.getElementAtIndex(5);

        mAccelerometerMxz = accelerometerMa.getElementAtIndex(6);
        mAccelerometerMyz = accelerometerMa.getElementAtIndex(7);
        mAccelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias ot be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(mInitialBiasX,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(mInitialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final AngularSpeed initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(mInitialBiasY,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(mInitialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final AngularSpeed initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(mInitialBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(mInitialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final AngularSpeed initialBiasZ)
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 1106
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 1108
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 2359
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasX() {
        return mAccelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final double accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasY() {
        return mAccelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final double accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasZ() {
        return mAccelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final double accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(mAccelerometerBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(mAccelerometerBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(mAccelerometerBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final double accelerometerBiasX,
            final double accelerometerBiasY,
            final double accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = accelerometerBiasX;
        mAccelerometerBiasY = accelerometerBiasY;
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public double[] getAccelerometerBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = mAccelerometerBiasX;
        result[1] = mAccelerometerBiasY;
        result[2] = mAccelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void setAccelerometerBias(final double[] accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias[0];
        mAccelerometerBiasY = accelerometerBias[1];
        mAccelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerBiasX);
        result.setElementAtIndex(1, mAccelerometerBiasY);
        result.setElementAtIndex(2, mAccelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setAccelerometerBias(final Matrix accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS
                || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        mAccelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        mAccelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    public double getAccelerometerSx() {
        return mAccelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSx(final double accelerometerSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    public double getAccelerometerSy() {
        return mAccelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSy(final double accelerometerSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    public double getAccelerometerSz() {
        return mAccelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSz(final double accelerometerSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    public double getAccelerometerMxy() {
        return mAccelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxy(final double accelerometerMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    public double getAccelerometerMxz() {
        return mAccelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxz(final double accelerometerMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    public double getAccelerometerMyx() {
        return mAccelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyx(final double accelerometerMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    public double getAccelerometerMyz() {
        return mAccelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyz(final double accelerometerMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    public double getAccelerometerMzx() {
        return mAccelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzx(final double accelerometerMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    public double getAccelerometerMzy() {
        return mAccelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzy(final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
        mAccelerometerSy = accelerometerSy;
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
        mAccelerometerMxz = accelerometerMxz;
        mAccelerometerMyx = accelerometerMyx;
        mAccelerometerMyz = accelerometerMyz;
        mAccelerometerMzx = accelerometerMzx;
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy,
                accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy,
                accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx,
                accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerSx);
        result.setElementAtIndex(1, mAccelerometerMyx);
        result.setElementAtIndex(2, mAccelerometerMzx);

        result.setElementAtIndex(3, mAccelerometerMxy);
        result.setElementAtIndex(4, mAccelerometerSy);
        result.setElementAtIndex(5, mAccelerometerMzy);

        result.setElementAtIndex(6, mAccelerometerMxz);
        result.setElementAtIndex(7, mAccelerometerMyz);
        result.setElementAtIndex(8, mAccelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setAccelerometerMa(final Matrix accelerometerMa)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerSx = accelerometerMa.getElementAtIndex(0);
        mAccelerometerMyx = accelerometerMa.getElementAtIndex(1);
        mAccelerometerMzx = accelerometerMa.getElementAtIndex(2);

        mAccelerometerMxy = accelerometerMa.getElementAtIndex(3);
        mAccelerometerSy = accelerometerMa.getElementAtIndex(4);
        mAccelerometerMzy = accelerometerMa.getElementAtIndex(5);

        mAccelerometerMxz = accelerometerMa.getElementAtIndex(6);
        mAccelerometerMyz = accelerometerMa.getElementAtIndex(7);
        mAccelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias ot be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(mInitialBiasX,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(mInitialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final AngularSpeed initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(mInitialBiasY,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(mInitialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final AngularSpeed initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(mInitialBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(mInitialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final AngularSpeed initialBiasZ)
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 5730
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1113
            final KnownBiasAndPositionAccelerometerCalibrationListener listener) {
        this(convertPosition(position), measurements, commonAxisUsed,
                bias, initialMa, listener);
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return x-coordinate of known accelerometer bias.
     */
    public double getBiasX() {
        return mBiasX;
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final double biasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return y-coordinate of known accelerometer bias.
     */
    public double getBiasY() {
        return mBiasY;
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final double biasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = biasY;
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return z-coordinate of known accelerometer bias.
     */
    public double getBiasZ() {
        return mBiasZ;
    }

    /**
     * Sets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final double biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = biasZ;
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @return x-coordinate of known accelerometer bias.
     */
    public Acceleration getBiasXAsAcceleration() {
        return new Acceleration(mBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final Acceleration biasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = convertAcceleration(biasX);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @return y-coordinate of known accelerometer bias.
     */
    public Acceleration getBiasYAsAcceleration() {
        return new Acceleration(mBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final Acceleration biasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = convertAcceleration(biasY);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @return z-coordinate of known accelerometer bias.
     */
    public Acceleration getBiasZAsAcceleration() {
        return new Acceleration(mBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets z-coordinate of known accelerometer bias to be used to find a solution.
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final Acceleration biasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Sets known bias coordinates of accelerometer expressed in meters
     * per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @param biasY y-coordinate of known accelerometer bias.
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(final double biasX, final double biasY,
                        final double biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
        mBiasY = biasY;
        mBiasZ = biasZ;
    }

    /**
     * Sets known bias coordinates of accelerometer.
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @param biasY y-coordinate of known accelerometer bias.
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(final Acceleration biasX,
                        final Acceleration biasY,
                        final Acceleration biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = convertAcceleration(biasX);
        mBiasY = convertAcceleration(biasY);
        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    public double[] getBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mBiasX;
        result[1] = mBiasY;
        result[2] = mBiasZ;
    }

    /**
     * Sets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known bias to be set
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setBias(final double[] bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mBiasX = bias[0];
        mBiasY = bias[1];
        mBiasZ = bias[2];
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @return known bias as a column matrix.
     */
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mBiasX);
        result.setElementAtIndex(1, mBiasY);
        result.setElementAtIndex(2, mBiasZ);
    }

    /**
     * Sets known bias as an array.
     *
     * @param bias bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setBias(final Matrix bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS
                || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mBiasX = bias.getElementAtIndex(0);
        mBiasY = bias.getElementAtIndex(1);
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS ||
                initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMa.getElementAtIndex(0);
        mInitialMyx = initialMa.getElementAtIndex(1);
        mInitialMzx = initialMa.getElementAtIndex(2);

        mInitialMxy = initialMa.getElementAtIndex(3);
        mInitialSy = initialMa.getElementAtIndex(4);
        mInitialMzy = initialMa.getElementAtIndex(5);

        mInitialMxz = initialMa.getElementAtIndex(6);
        mInitialMyz = initialMa.getElementAtIndex(7);
        mInitialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return mPosition;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final NEDPosition result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (mPosition != null) {
            final NEDVelocity velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    mPosition.getX(), mPosition.getY(), mPosition.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6170
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1130
            final KnownPositionAccelerometerCalibratorListener listener) {
        this(convertPosition(position), measurements, commonAxisUsed,
                initialBias, initialMa, listener);
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    public Acceleration getInitialBiasXAsAcceleration() {
        return new Acceleration(mInitialBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mInitialBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final Acceleration initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAcceleration(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    public Acceleration getInitialBiasYAsAcceleration() {
        return new Acceleration(mInitialBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mInitialBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final Acceleration initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = convertAcceleration(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    public Acceleration getInitialBiasZAsAcceleration() {
        return new Acceleration(mInitialBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mInitialBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final Acceleration initialBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution
     * expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(final double initialBiasX, final double initialBiasY,
                               final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
        mInitialBiasY = initialBiasY;
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(final Acceleration initialBiasX,
                               final Acceleration initialBiasY,
                               final Acceleration initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAcceleration(initialBiasX);
        mInitialBiasY = convertAcceleration(initialBiasY);
        mInitialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of initial bias.
     */
    public double[] getInitialBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mInitialBiasX;
        result[1] = mInitialBiasY;
        result[2] = mInitialBiasZ;
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mInitialBiasX = initialBias[0];
        mInitialBiasY = initialBias[1];
        mInitialBiasZ = initialBias[2];
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     *
     * @return initial bias to be used to find a solution as a column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialBiasX);
        result.setElementAtIndex(1, mInitialBiasY);
        result.setElementAtIndex(2, mInitialBiasZ);
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS
                || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mInitialBiasX = initialBias.getElementAtIndex(0);
        mInitialBiasY = initialBias.getElementAtIndex(1);
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS ||
                initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMa.getElementAtIndex(0);
        mInitialMyx = initialMa.getElementAtIndex(1);
        mInitialMzx = initialMa.getElementAtIndex(2);

        mInitialMxy = initialMa.getElementAtIndex(3);
        mInitialSy = initialMa.getElementAtIndex(4);
        mInitialMzy = initialMa.getElementAtIndex(5);

        mInitialMxz = initialMa.getElementAtIndex(6);
        mInitialMyz = initialMa.getElementAtIndex(7);
        mInitialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return mPosition;
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final NEDPosition result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (mPosition != null) {
            final NEDVelocity velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    mPosition.getX(), mPosition.getY(), mPosition.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 1889
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 1891
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 490
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias ot be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(mInitialBiasX,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(mInitialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final AngularSpeed initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(mInitialBiasY,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(mInitialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final AngularSpeed initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(mInitialBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(mInitialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final AngularSpeed initialBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution
     * expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final double initialBiasX, final double initialBiasY,
            final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
        mInitialBiasY = initialBiasY;
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final AngularSpeed initialBiasX,
            final AngularSpeed initialBiasY,
            final AngularSpeed initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAngularSpeed(initialBiasX);
        mInitialBiasY = convertAngularSpeed(initialBiasY);
        mInitialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy,
            final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of initial gyroscope bias.
     */
    public double[] getInitialBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mInitialBiasX;
        result[1] = mInitialBiasY;
        result[2] = mInitialBiasZ;
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mInitialBiasX = initialBias[0];
        mInitialBiasY = initialBias[1];
        mInitialBiasZ = initialBias[2];
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialBiasX);
        result.setElementAtIndex(1, mInitialBiasY);
        result.setElementAtIndex(2, mInitialBiasZ);
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * a column matrix.
     *
     * @param initialBias initial gyroscope bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS
                || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mInitialBiasX = initialBias.getElementAtIndex(0);
        mInitialBiasY = initialBias.getElementAtIndex(1);
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 490
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3097
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a solutions.
     * This is expressed in radians per second (rad/s) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(mInitialBiasX,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(mInitialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final AngularSpeed initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(mInitialBiasY,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(mInitialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final AngularSpeed initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(mInitialBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(mInitialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final AngularSpeed initialBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution
     * expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(final double initialBiasX, final double initialBiasY,
                               final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
        mInitialBiasY = initialBiasY;
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(final AngularSpeed initialBiasX,
                               final AngularSpeed initialBiasY,
                               final AngularSpeed initialBiasZ)
            throws LockedException {

        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAngularSpeed(initialBiasX);
        mInitialBiasY = convertAngularSpeed(initialBiasY);
        mInitialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of initial bias.
     */
    public double[] getInitialBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mInitialBiasX;
        result[1] = mInitialBiasY;
        result[2] = mInitialBiasZ;
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mInitialBiasX = initialBias[0];
        mInitialBiasY = initialBias[1];
        mInitialBiasZ = initialBias[2];
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     *
     * @return initial bias to be used to find a solution as a column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialBiasX);
        result.setElementAtIndex(1, mInitialBiasY);
        result.setElementAtIndex(2, mInitialBiasZ);
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS
                || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mInitialBiasX = initialBias.getElementAtIndex(0);
        mInitialBiasY = initialBias.getElementAtIndex(1);
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    public List<StandardDeviationFrameBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1204
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1275
            final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
        this(convertPosition(position), measurements, commonAxisUsed,
                hardIron, initialMm, listener);
    }

    /**
     * Gets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known x-coordinate of magnetometer hard-iron bias.
     */
    public double getHardIronX() {
        return mHardIronX;
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronX known x-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setHardIronX(final double hardIronX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mHardIronX = hardIronX;
    }

    /**
     * Gets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known y-coordinate of magnetometer hard-iron bias.
     */
    public double getHardIronY() {
        return mHardIronY;
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronY known y-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setHardIronY(final double hardIronY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mHardIronY = hardIronY;
    }

    /**
     * Gets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known z-coordinate of magnetometer hard-iron bias.
     */
    public double getHardIronZ() {
        return mHardIronZ;
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in meters Teslas (T).
     *
     * @param hardIronZ known z-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setHardIronZ(final double hardIronZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mHardIronZ = hardIronZ;
    }

    /**
     * Sets known hard-iron bias coordinates of magnetometer expressed in
     * Teslas (T).
     *
     * @param hardIronX x-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronY y-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronZ z-coordinate of magnetometer
     *                  known hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setHardIron(
            final double hardIronX, final double hardIronY,
            final double hardIronZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mHardIronX = hardIronX;
        mHardIronY = hardIronY;
        mHardIronZ = hardIronZ;
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx,
            final double initialSy,
            final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy,
            final double initialMxz,
            final double initialMyx,
            final double initialMyz,
            final double initialMzx,
            final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx,
            final double initialSy,
            final double initialSz,
            final double initialMxy,
            final double initialMxz,
            final double initialMyx,
            final double initialMyz,
            final double initialMzx,
            final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of known bias.
     */
    public double[] getHardIron() {
        final double[] result = new double[
                BodyMagneticFluxDensity.COMPONENTS];
        getHardIron(result);
        return result;
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mHardIronX;
        result[1] = mHardIronY;
        result[2] = mHardIronZ;
    }

    /**
     * Sets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param hardIron known hard-iron.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setHardIron(final double[] hardIron)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mHardIronX = hardIron[0];
        mHardIronY = hardIron[1];
        mHardIronZ = hardIron[2];
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @return known hard-iron bias as a column matrix.
     */
    public Matrix getHardIronAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    1);
            getHardIronAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getHardIronAsMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mHardIronX);
        result.setElementAtIndex(1, mHardIronY);
        result.setElementAtIndex(2, mHardIronZ);
    }

    /**
     * Sets known hard-iron bias as a column matrix.
     *
     * @param hardIron known hard-iron bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setHardIron(final Matrix hardIron)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS
                || hardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mHardIronX = hardIron.getElementAtIndex(0);
        mHardIronY = hardIron.getElementAtIndex(1);
        mHardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMm(final Matrix initialMm)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS ||
                initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMm.getElementAtIndex(0);
        mInitialMyx = initialMm.getElementAtIndex(1);
        mInitialMzx = initialMm.getElementAtIndex(2);

        mInitialMxy = initialMm.getElementAtIndex(3);
        mInitialSy = initialMm.getElementAtIndex(4);
        mInitialMzy = initialMm.getElementAtIndex(5);

        mInitialMxz = initialMm.getElementAtIndex(6);
        mInitialMyz = initialMm.getElementAtIndex(7);
        mInitialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken.
     *
     * @return position where body magnetic flux density measurements have
     * been taken.
     */
    public NEDPosition getNedPosition() {
        return mPosition;
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken.
     *
     * @param position position where body magnetic flux density measurements
     *                 have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @return position where body magnetic flux density measurements have
     * been taken or null if not available.
     */
    public ECEFPosition getEcefPosition() {
        if (mPosition != null) {
            final ECEFPosition result = new ECEFPosition();
            getEcefPosition(result);
            return result;
        } else {
            return null;
        }
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if ECEF position could be computed, false otherwise.
     */
    public boolean getEcefPosition(final ECEFPosition result) {

        if (mPosition != null) {
            final ECEFVelocity velocity = new ECEFVelocity();
            NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                    mPosition.getLatitude(), mPosition.getLongitude(),
                    mPosition.getHeight(), 0.0, 0.0, 0.0,
                    result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param position position where body magnetic flux density have been
     *                 taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Gets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @return timestamp expressed as decimal year or null if not defined.
     */
    public Double getYear() {
        return mYear;
    }

    /**
     * Sets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @param year timestamp expressed as decimal year.
     * @throws LockedException if calibrator is currently running.
     */
    public void setYear(final Double year) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = year;
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param timestampMillis a timestamp expressed in milliseocnds since
     *                        epoch time (January 1st, 1970 at midnight).
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Long timestampMillis) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(timestampMillis);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param date a date instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Date date) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(date);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param calendar a calendar instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final GregorianCalendar calendar)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(calendar);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 1210
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1281
            final KnownPositionAndInstantMagnetometerCalibratorListener listener) {
        this(convertPosition(position), measurements, commonAxisUsed,
                initialHardIron, initialMm, listener);
    }

    /**
     * Gets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    public double getInitialHardIronX() {
        return mInitialHardIronX;
    }

    /**
     * Sets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialHardIronX(final double initialHardIronX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialHardIronX = initialHardIronX;
    }

    /**
     * Gets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    public double getInitialHardIronY() {
        return mInitialHardIronY;
    }

    /**
     * Sets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialHardIronY(final double initialHardIronY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialHardIronY = initialHardIronY;
    }

    /**
     * Gets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    public double getInitialHardIronZ() {
        return mInitialHardIronZ;
    }

    /**
     * Sets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in meters Teslas (T).
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialHardIronZ(final double initialHardIronZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialHardIronZ = initialHardIronZ;
    }

    /**
     * Sets initial hard-iron bias coordinates of magnetometer used to find
     * a solution expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialHardIron(
            final double initialHardIronX, final double initialHardIronY,
            final double initialHardIronZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialHardIronX = initialHardIronX;
        mInitialHardIronY = initialHardIronY;
        mInitialHardIronZ = initialHardIronZ;
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx,
            final double initialSy,
            final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy,
            final double initialMxz,
            final double initialMyx,
            final double initialMyz,
            final double initialMzx,
            final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx,
            final double initialSy,
            final double initialSz,
            final double initialMxy,
            final double initialMxz,
            final double initialMyx,
            final double initialMyz,
            final double initialMzx,
            final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of initial bias.
     */
    public double[] getInitialHardIron() {
        final double[] result = new double[
                BodyMagneticFluxDensity.COMPONENTS];
        getInitialHardIron(result);
        return result;
    }

    /**
     * Gets initial hard-iron  bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getInitialHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mInitialHardIronX;
        result[1] = mInitialHardIronY;
        result[2] = mInitialHardIronZ;
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialHardIron(final double[] initialHardIron)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mInitialHardIronX = initialHardIron[0];
        mInitialHardIronY = initialHardIron[1];
        mInitialHardIronZ = initialHardIron[2];
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     *
     * @return initial hard-iron bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getInitialHardIronAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    1);
            getInitialHardIronAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialHardIronAsMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialHardIronX);
        result.setElementAtIndex(1, mInitialHardIronY);
        result.setElementAtIndex(2, mInitialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as a column
     * matrix.
     *
     * @param initialHardIron initial hard-iron bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialHardIron(final Matrix initialHardIron)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS
                || initialHardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mInitialHardIronX = initialHardIron.getElementAtIndex(0);
        mInitialHardIronY = initialHardIron.getElementAtIndex(1);
        mInitialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMm(final Matrix initialMm)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS ||
                initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMm.getElementAtIndex(0);
        mInitialMyx = initialMm.getElementAtIndex(1);
        mInitialMzx = initialMm.getElementAtIndex(2);

        mInitialMxy = initialMm.getElementAtIndex(3);
        mInitialSy = initialMm.getElementAtIndex(4);
        mInitialMzy = initialMm.getElementAtIndex(5);

        mInitialMxz = initialMm.getElementAtIndex(6);
        mInitialMyz = initialMm.getElementAtIndex(7);
        mInitialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken.
     *
     * @return position where body magnetic flux density measurements have
     * been taken.
     */
    public NEDPosition getNedPosition() {
        return mPosition;
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken.
     *
     * @param position position where body magnetic flux density measurements
     *                 have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @return position where body magnetic flux density measurements have
     * been taken or null if not available.
     */
    public ECEFPosition getEcefPosition() {
        if (mPosition != null) {
            final ECEFPosition result = new ECEFPosition();
            getEcefPosition(result);
            return result;
        } else {
            return null;
        }
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if ECEF position could be computed, false otherwise.
     */
    public boolean getEcefPosition(final ECEFPosition result) {

        if (mPosition != null) {
            final ECEFVelocity velocity = new ECEFVelocity();
            NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                    mPosition.getLatitude(), mPosition.getLongitude(),
                    mPosition.getHeight(), 0.0, 0.0, 0.0,
                    result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param position position where body magnetic flux density have been
     *                 taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Gets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @return timestamp expressed as decimal year or null if not defined.
     */
    public Double getYear() {
        return mYear;
    }

    /**
     * Sets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @param year timestamp expressed as decimal year.
     * @throws LockedException if calibrator is currently running.
     */
    public void setYear(final Double year) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = year;
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param timestampMillis a timestamp expressed in milliseocnds since
     *                        epoch time (January 1st, 1970 at midnight).
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Long timestampMillis) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(timestampMillis);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param date a date instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Date date) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(date);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param calendar a calendar instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final GregorianCalendar calendar)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(calendar);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3288
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3333
        mInitialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution
     * expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final double initialBiasX, final double initialBiasY,
            final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
        mInitialBiasY = initialBiasY;
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final AngularSpeed initialBiasX,
            final AngularSpeed initialBiasY,
            final AngularSpeed initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAngularSpeed(initialBiasX);
        mInitialBiasY = convertAngularSpeed(initialBiasY);
        mInitialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy,
            final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of initial gyroscope bias.
     */
    public double[] getInitialBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mInitialBiasX;
        result[1] = mInitialBiasY;
        result[2] = mInitialBiasZ;
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mInitialBiasX = initialBias[0];
        mInitialBiasY = initialBias[1];
        mInitialBiasZ = initialBias[2];
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialBiasX);
        result.setElementAtIndex(1, mInitialBiasY);
        result.setElementAtIndex(2, mInitialBiasZ);
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * an array.
     *
     * @param initialBias initial gyroscope bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS
                || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mInitialBiasX = initialBias.getElementAtIndex(0);
        mInitialBiasY = initialBias.getElementAtIndex(1);
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @return constant rotation rate of turntable.
     */
    public double getTurntableRotationRate() {
        return mTurntableRotationRate;
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final double turntableRotationRate) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (turntableRotationRate <= 0.0) {
            throw new IllegalArgumentException();
        }

        mTurntableRotationRate = turntableRotationRate;
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @return constant rotation rate of turntable.
     */
    public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
        return new AngularSpeed(mTurntableRotationRate,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @param result instance where result will be stored.
     */
    public void getTurntableRotationRateAsAngularSpeed(
            final AngularSpeed result) {
        result.setValue(mTurntableRotationRate);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final AngularSpeed turntableRotationRate)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setTurntableRotationRate(
                convertAngularSpeed(turntableRotationRate));
    }

    /**
     * Gets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @return time interval between measurements.
     */
    public double getTimeInterval() {
        return mTimeInterval;
    }

    /**
     * Sets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTimeInterval(final double timeInterval)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (timeInterval <= 0.0) {
            throw new IllegalArgumentException();
        }
        mTimeInterval = timeInterval;
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @return time interval between measurements.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(mTimeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @param result instance where result will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(mTimeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between measurements being captured.
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTimeInterval(final Time timeInterval)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setTimeInterval(convertTime(timeInterval));
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    public List<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 1103
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1081
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 1105
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1083
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa);
        mListener = listener;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasX() {
        return mAccelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final double accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasY() {
        return mAccelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final double accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasZ() {
        return mAccelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final double accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(mAccelerometerBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(mAccelerometerBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(mAccelerometerBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final double accelerometerBiasX,
            final double accelerometerBiasY,
            final double accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = accelerometerBiasX;
        mAccelerometerBiasY = accelerometerBiasY;
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public double[] getAccelerometerBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = mAccelerometerBiasX;
        result[1] = mAccelerometerBiasY;
        result[2] = mAccelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void setAccelerometerBias(final double[] accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias[0];
        mAccelerometerBiasY = accelerometerBias[1];
        mAccelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerBiasX);
        result.setElementAtIndex(1, mAccelerometerBiasY);
        result.setElementAtIndex(2, mAccelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setAccelerometerBias(final Matrix accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS
                || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        mAccelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        mAccelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    public double getAccelerometerSx() {
        return mAccelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSx(final double accelerometerSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    public double getAccelerometerSy() {
        return mAccelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSy(final double accelerometerSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    public double getAccelerometerSz() {
        return mAccelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSz(final double accelerometerSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    public double getAccelerometerMxy() {
        return mAccelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxy(final double accelerometerMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    public double getAccelerometerMxz() {
        return mAccelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxz(final double accelerometerMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    public double getAccelerometerMyx() {
        return mAccelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyx(final double accelerometerMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    public double getAccelerometerMyz() {
        return mAccelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyz(final double accelerometerMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    public double getAccelerometerMzx() {
        return mAccelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzx(final double accelerometerMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    public double getAccelerometerMzy() {
        return mAccelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzy(final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
        mAccelerometerSy = accelerometerSy;
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
        mAccelerometerMxz = accelerometerMxz;
        mAccelerometerMyx = accelerometerMyx;
        mAccelerometerMyz = accelerometerMyz;
        mAccelerometerMzx = accelerometerMzx;
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy,
                accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy,
                accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx,
                accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerSx);
        result.setElementAtIndex(1, mAccelerometerMyx);
        result.setElementAtIndex(2, mAccelerometerMzx);

        result.setElementAtIndex(3, mAccelerometerMxy);
        result.setElementAtIndex(4, mAccelerometerSy);
        result.setElementAtIndex(5, mAccelerometerMzy);

        result.setElementAtIndex(6, mAccelerometerMxz);
        result.setElementAtIndex(7, mAccelerometerMyz);
        result.setElementAtIndex(8, mAccelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setAccelerometerMa(final Matrix accelerometerMa)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerSx = accelerometerMa.getElementAtIndex(0);
        mAccelerometerMyx = accelerometerMa.getElementAtIndex(1);
        mAccelerometerMzx = accelerometerMa.getElementAtIndex(2);

        mAccelerometerMxy = accelerometerMa.getElementAtIndex(3);
        mAccelerometerSy = accelerometerMa.getElementAtIndex(4);
        mAccelerometerMzy = accelerometerMa.getElementAtIndex(5);

        mAccelerometerMxz = accelerometerMa.getElementAtIndex(6);
        mAccelerometerMyz = accelerometerMa.getElementAtIndex(7);
        mAccelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 2314
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 2274
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 2312
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 2357
                bias, initialMg, initialGg, accelerometerBias,
                accelerometerMa, listener);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasX() {
        return mAccelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final double accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasY() {
        return mAccelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final double accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasZ() {
        return mAccelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final double accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(mAccelerometerBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(mAccelerometerBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(mAccelerometerBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final double accelerometerBiasX,
            final double accelerometerBiasY,
            final double accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = accelerometerBiasX;
        mAccelerometerBiasY = accelerometerBiasY;
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public double[] getAccelerometerBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = mAccelerometerBiasX;
        result[1] = mAccelerometerBiasY;
        result[2] = mAccelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void setAccelerometerBias(final double[] accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias[0];
        mAccelerometerBiasY = accelerometerBias[1];
        mAccelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerBiasX);
        result.setElementAtIndex(1, mAccelerometerBiasY);
        result.setElementAtIndex(2, mAccelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setAccelerometerBias(final Matrix accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS
                || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        mAccelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        mAccelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    public double getAccelerometerSx() {
        return mAccelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSx(final double accelerometerSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    public double getAccelerometerSy() {
        return mAccelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSy(final double accelerometerSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    public double getAccelerometerSz() {
        return mAccelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSz(final double accelerometerSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    public double getAccelerometerMxy() {
        return mAccelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxy(final double accelerometerMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    public double getAccelerometerMxz() {
        return mAccelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxz(final double accelerometerMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    public double getAccelerometerMyx() {
        return mAccelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyx(final double accelerometerMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    public double getAccelerometerMyz() {
        return mAccelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyz(final double accelerometerMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    public double getAccelerometerMzx() {
        return mAccelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzx(final double accelerometerMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    public double getAccelerometerMzy() {
        return mAccelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzy(final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
        mAccelerometerSy = accelerometerSy;
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
        mAccelerometerMxz = accelerometerMxz;
        mAccelerometerMyx = accelerometerMyx;
        mAccelerometerMyz = accelerometerMyz;
        mAccelerometerMzx = accelerometerMzx;
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy,
                accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy,
                accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx,
                accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerSx);
        result.setElementAtIndex(1, mAccelerometerMyx);
        result.setElementAtIndex(2, mAccelerometerMzx);

        result.setElementAtIndex(3, mAccelerometerMxy);
        result.setElementAtIndex(4, mAccelerometerSy);
        result.setElementAtIndex(5, mAccelerometerMzy);

        result.setElementAtIndex(6, mAccelerometerMxz);
        result.setElementAtIndex(7, mAccelerometerMyz);
        result.setElementAtIndex(8, mAccelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setAccelerometerMa(final Matrix accelerometerMa)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerSx = accelerometerMa.getElementAtIndex(0);
        mAccelerometerMyx = accelerometerMa.getElementAtIndex(1);
        mAccelerometerMzx = accelerometerMa.getElementAtIndex(2);

        mAccelerometerMxy = accelerometerMa.getElementAtIndex(3);
        mAccelerometerSy = accelerometerMa.getElementAtIndex(4);
        mAccelerometerMzy = accelerometerMa.getElementAtIndex(5);

        mAccelerometerMxz = accelerometerMa.getElementAtIndex(6);
        mAccelerometerMyz = accelerometerMa.getElementAtIndex(7);
        mAccelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets known x-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return known x-coordinate of gyroscope bias.
     */
    public double getBiasX() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 1106
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 2316
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 1108
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 2276
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasX() {
        return mAccelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final double accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasY() {
        return mAccelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final double accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasZ() {
        return mAccelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final double accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(mAccelerometerBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(mAccelerometerBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(mAccelerometerBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final double accelerometerBiasX,
            final double accelerometerBiasY,
            final double accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = accelerometerBiasX;
        mAccelerometerBiasY = accelerometerBiasY;
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public double[] getAccelerometerBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = mAccelerometerBiasX;
        result[1] = mAccelerometerBiasY;
        result[2] = mAccelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void setAccelerometerBias(final double[] accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias[0];
        mAccelerometerBiasY = accelerometerBias[1];
        mAccelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerBiasX);
        result.setElementAtIndex(1, mAccelerometerBiasY);
        result.setElementAtIndex(2, mAccelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setAccelerometerBias(final Matrix accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS
                || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        mAccelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        mAccelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    public double getAccelerometerSx() {
        return mAccelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSx(final double accelerometerSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    public double getAccelerometerSy() {
        return mAccelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSy(final double accelerometerSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    public double getAccelerometerSz() {
        return mAccelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSz(final double accelerometerSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    public double getAccelerometerMxy() {
        return mAccelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxy(final double accelerometerMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    public double getAccelerometerMxz() {
        return mAccelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxz(final double accelerometerMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    public double getAccelerometerMyx() {
        return mAccelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyx(final double accelerometerMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    public double getAccelerometerMyz() {
        return mAccelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyz(final double accelerometerMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    public double getAccelerometerMzx() {
        return mAccelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzx(final double accelerometerMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    public double getAccelerometerMzy() {
        return mAccelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzy(final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
        mAccelerometerSy = accelerometerSy;
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
        mAccelerometerMxz = accelerometerMxz;
        mAccelerometerMyx = accelerometerMyx;
        mAccelerometerMyz = accelerometerMyz;
        mAccelerometerMzx = accelerometerMzx;
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy,
                accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy,
                accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx,
                accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerSx);
        result.setElementAtIndex(1, mAccelerometerMyx);
        result.setElementAtIndex(2, mAccelerometerMzx);

        result.setElementAtIndex(3, mAccelerometerMxy);
        result.setElementAtIndex(4, mAccelerometerSy);
        result.setElementAtIndex(5, mAccelerometerMzy);

        result.setElementAtIndex(6, mAccelerometerMxz);
        result.setElementAtIndex(7, mAccelerometerMyz);
        result.setElementAtIndex(8, mAccelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setAccelerometerMa(final Matrix accelerometerMa)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerSx = accelerometerMa.getElementAtIndex(0);
        mAccelerometerMyx = accelerometerMa.getElementAtIndex(1);
        mAccelerometerMzx = accelerometerMa.getElementAtIndex(2);

        mAccelerometerMxy = accelerometerMa.getElementAtIndex(3);
        mAccelerometerSy = accelerometerMa.getElementAtIndex(4);
        mAccelerometerMzy = accelerometerMa.getElementAtIndex(5);

        mAccelerometerMxz = accelerometerMa.getElementAtIndex(6);
        mAccelerometerMyz = accelerometerMa.getElementAtIndex(7);
        mAccelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1084
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1086
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 2314
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 2359
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasX() {
        return mAccelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final double accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasY() {
        return mAccelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final double accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public double getAccelerometerBiasZ() {
        return mAccelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final double accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(mAccelerometerBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(mAccelerometerBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(mAccelerometerBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final double accelerometerBiasX,
            final double accelerometerBiasY,
            final double accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = accelerometerBiasX;
        mAccelerometerBiasY = accelerometerBiasY;
        mAccelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mAccelerometerBiasX = convertAcceleration(accelerometerBiasX);
        mAccelerometerBiasY = convertAcceleration(accelerometerBiasY);
        mAccelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public double[] getAccelerometerBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = mAccelerometerBiasX;
        result[1] = mAccelerometerBiasY;
        result[2] = mAccelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void setAccelerometerBias(final double[] accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias[0];
        mAccelerometerBiasY = accelerometerBias[1];
        mAccelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerBiasX);
        result.setElementAtIndex(1, mAccelerometerBiasY);
        result.setElementAtIndex(2, mAccelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setAccelerometerBias(final Matrix accelerometerBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS
                || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mAccelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        mAccelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        mAccelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    public double getAccelerometerSx() {
        return mAccelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSx(final double accelerometerSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    public double getAccelerometerSy() {
        return mAccelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSy(final double accelerometerSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    public double getAccelerometerSz() {
        return mAccelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerSz(final double accelerometerSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    public double getAccelerometerMxy() {
        return mAccelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxy(final double accelerometerMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    public double getAccelerometerMxz() {
        return mAccelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMxz(final double accelerometerMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    public double getAccelerometerMyx() {
        return mAccelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyx(final double accelerometerMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    public double getAccelerometerMyz() {
        return mAccelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMyz(final double accelerometerMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    public double getAccelerometerMzx() {
        return mAccelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzx(final double accelerometerMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    public double getAccelerometerMzy() {
        return mAccelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerMzy(final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerSx = accelerometerSx;
        mAccelerometerSy = accelerometerSy;
        mAccelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mAccelerometerMxy = accelerometerMxy;
        mAccelerometerMxz = accelerometerMxz;
        mAccelerometerMyx = accelerometerMyx;
        mAccelerometerMyz = accelerometerMyz;
        mAccelerometerMzx = accelerometerMzx;
        mAccelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy,
                accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy,
                accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx,
                accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mAccelerometerSx);
        result.setElementAtIndex(1, mAccelerometerMyx);
        result.setElementAtIndex(2, mAccelerometerMzx);

        result.setElementAtIndex(3, mAccelerometerMxy);
        result.setElementAtIndex(4, mAccelerometerSy);
        result.setElementAtIndex(5, mAccelerometerMzy);

        result.setElementAtIndex(6, mAccelerometerMxz);
        result.setElementAtIndex(7, mAccelerometerMyz);
        result.setElementAtIndex(8, mAccelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setAccelerometerMa(final Matrix accelerometerMa)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mAccelerometerSx = accelerometerMa.getElementAtIndex(0);
        mAccelerometerMyx = accelerometerMa.getElementAtIndex(1);
        mAccelerometerMzx = accelerometerMa.getElementAtIndex(2);

        mAccelerometerMxy = accelerometerMa.getElementAtIndex(3);
        mAccelerometerSy = accelerometerMa.getElementAtIndex(4);
        mAccelerometerMzy = accelerometerMa.getElementAtIndex(5);

        mAccelerometerMxz = accelerometerMa.getElementAtIndex(6);
        mAccelerometerMyz = accelerometerMa.getElementAtIndex(7);
        mAccelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return x-coordinate of gyroscope known bias.
     */
    public double getBiasX() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6173
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 461
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1133
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    public Acceleration getInitialBiasXAsAcceleration() {
        return new Acceleration(mInitialBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mInitialBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final Acceleration initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAcceleration(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    public Acceleration getInitialBiasYAsAcceleration() {
        return new Acceleration(mInitialBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mInitialBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final Acceleration initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = convertAcceleration(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    public Acceleration getInitialBiasZAsAcceleration() {
        return new Acceleration(mInitialBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mInitialBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final Acceleration initialBiasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution
     * expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(final double initialBiasX, final double initialBiasY,
                               final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
        mInitialBiasY = initialBiasY;
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(final Acceleration initialBiasX,
                               final Acceleration initialBiasY,
                               final Acceleration initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAcceleration(initialBiasX);
        mInitialBiasY = convertAcceleration(initialBiasY);
        mInitialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of initial bias.
     */
    public double[] getInitialBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mInitialBiasX;
        result[1] = mInitialBiasY;
        result[2] = mInitialBiasZ;
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mInitialBiasX = initialBias[0];
        mInitialBiasY = initialBias[1];
        mInitialBiasZ = initialBias[2];
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     *
     * @return initial bias to be used to find a solution as a column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialBiasX);
        result.setElementAtIndex(1, mInitialBiasY);
        result.setElementAtIndex(2, mInitialBiasZ);
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS
                || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mInitialBiasX = initialBias.getElementAtIndex(0);
        mInitialBiasY = initialBias.getElementAtIndex(1);
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS ||
                initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMa.getElementAtIndex(0);
        mInitialMyx = initialMa.getElementAtIndex(1);
        mInitialMzx = initialMa.getElementAtIndex(2);

        mInitialMxy = initialMa.getElementAtIndex(3);
        mInitialSy = initialMa.getElementAtIndex(4);
        mInitialMzy = initialMa.getElementAtIndex(5);

        mInitialMxz = initialMa.getElementAtIndex(6);
        mInitialMyz = initialMa.getElementAtIndex(7);
        mInitialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2080
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2082
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 669
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3333
        mInitialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution
     * expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final double initialBiasX, final double initialBiasY,
            final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
        mInitialBiasY = initialBiasY;
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final AngularSpeed initialBiasX,
            final AngularSpeed initialBiasY,
            final AngularSpeed initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAngularSpeed(initialBiasX);
        mInitialBiasY = convertAngularSpeed(initialBiasY);
        mInitialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy,
            final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of initial gyroscope bias.
     */
    public double[] getInitialBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mInitialBiasX;
        result[1] = mInitialBiasY;
        result[2] = mInitialBiasZ;
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mInitialBiasX = initialBias[0];
        mInitialBiasY = initialBias[1];
        mInitialBiasZ = initialBias[2];
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialBiasX);
        result.setElementAtIndex(1, mInitialBiasY);
        result.setElementAtIndex(2, mInitialBiasZ);
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * a column matrix.
     *
     * @param initialBias initial gyroscope bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS
                || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mInitialBiasX = initialBias.getElementAtIndex(0);
        mInitialBiasY = initialBias.getElementAtIndex(1);
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 1213
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 480
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1284
    }

    /**
     * Gets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    public double getInitialHardIronX() {
        return mInitialHardIronX;
    }

    /**
     * Sets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialHardIronX(final double initialHardIronX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialHardIronX = initialHardIronX;
    }

    /**
     * Gets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    public double getInitialHardIronY() {
        return mInitialHardIronY;
    }

    /**
     * Sets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialHardIronY(final double initialHardIronY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialHardIronY = initialHardIronY;
    }

    /**
     * Gets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    public double getInitialHardIronZ() {
        return mInitialHardIronZ;
    }

    /**
     * Sets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in meters Teslas (T).
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialHardIronZ(final double initialHardIronZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialHardIronZ = initialHardIronZ;
    }

    /**
     * Sets initial hard-iron bias coordinates of magnetometer used to find
     * a solution expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialHardIron(
            final double initialHardIronX, final double initialHardIronY,
            final double initialHardIronZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialHardIronX = initialHardIronX;
        mInitialHardIronY = initialHardIronY;
        mInitialHardIronZ = initialHardIronZ;
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx,
            final double initialSy,
            final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy,
            final double initialMxz,
            final double initialMyx,
            final double initialMyz,
            final double initialMzx,
            final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx,
            final double initialSy,
            final double initialSz,
            final double initialMxy,
            final double initialMxz,
            final double initialMyx,
            final double initialMyz,
            final double initialMzx,
            final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of initial bias.
     */
    public double[] getInitialHardIron() {
        final double[] result = new double[
                BodyMagneticFluxDensity.COMPONENTS];
        getInitialHardIron(result);
        return result;
    }

    /**
     * Gets initial hard-iron  bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getInitialHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mInitialHardIronX;
        result[1] = mInitialHardIronY;
        result[2] = mInitialHardIronZ;
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialHardIron(final double[] initialHardIron)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mInitialHardIronX = initialHardIron[0];
        mInitialHardIronY = initialHardIron[1];
        mInitialHardIronZ = initialHardIron[2];
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     *
     * @return initial hard-iron bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getInitialHardIronAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    1);
            getInitialHardIronAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialHardIronAsMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialHardIronX);
        result.setElementAtIndex(1, mInitialHardIronY);
        result.setElementAtIndex(2, mInitialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as a column
     * matrix.
     *
     * @param initialHardIron initial hard-iron bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialHardIron(final Matrix initialHardIron)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS
                || initialHardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mInitialHardIronX = initialHardIron.getElementAtIndex(0);
        mInitialHardIronY = initialHardIron.getElementAtIndex(1);
        mInitialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMm(final Matrix initialMm)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS ||
                initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMm.getElementAtIndex(0);
        mInitialMyx = initialMm.getElementAtIndex(1);
        mInitialMzx = initialMm.getElementAtIndex(2);

        mInitialMxy = initialMm.getElementAtIndex(3);
        mInitialSy = initialMm.getElementAtIndex(4);
        mInitialMzy = initialMm.getElementAtIndex(5);

        mInitialMxz = initialMm.getElementAtIndex(6);
        mInitialMyz = initialMm.getElementAtIndex(7);
        mInitialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken.
     *
     * @return position where body magnetic flux density measurements have
     * been taken.
     */
    public NEDPosition getNedPosition() {
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 1193
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 1366
    }

    /**
     * Gets Kalman filter error covariance matrix.
     * Notice that covariance is expressed in terms of ECEF coordinates.
     * If accuracy of position, attitude or velocity needs to be expressed in terms
     * of NED coordinates, their respective submatrices of this covariance matrix
     * must be rotated, taking into account the Jacobian of the matrix transformation
     * relating both coordinates, the covariance can be expressed following the law
     * of propagation of uncertainties (https://en.wikipedia.org/wiki/Propagation_of_uncertainty)
     * as: cov(f(x)) = J*cov(x)*J'.
     *
     * @param result instance where result data will be copied to.
     * @return true if result data has been copied, false otherwise.
     */
    public boolean getCovariance(final Matrix result) {
        if (mCovariance != null) {
            mCovariance.copyTo(result);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets Kalman filter error covariance matrix.
     * Notice that covariance is expressed in terms of ECEF coordinates.
     * If accuracy of position, attitude or velocity needs to be expressed in terms
     * of NED coordinates, their respective submatrices of this covariance matrix
     * must be rotated, taking into account the Jacobian of the matrix transformation
     * relating both coordinates, the covariance can be expressed following the law
     * of propagation of uncertainties (https://en.wikipedia.org/wiki/Propagation_of_uncertainty)
     * as: cov(f(x)) = J*cov(x)*J'.
     *
     * @return Kalman filter error covariance matrix.
     */
    public Matrix getCovariance() {
        return mCovariance;
    }

    /**
     * Sets Kalman filter error covariance matrix.
     *
     * @param covariance Kalman filter error covariance matrix to be set.
     * @throws IllegalArgumentException if provided covariance matrix is not 15x15.
     */
    public void setCovariance(final Matrix covariance) {
        if (covariance.getRows() != NUM_PARAMS ||
                covariance.getColumns() != NUM_PARAMS) {
            throw new IllegalArgumentException();
        }

        mCovariance = covariance;
    }

    /**
     * Gets body to ECEF coordinate transformation.
     *
     * @return body to ECEF coordinate transformation.
     * @throws InvalidRotationMatrixException if current body to ECEF transformation matrix is
     *                                        not valid (is not a 3x3 orthonormal matrix).
     */
    public CoordinateTransformation getC() throws InvalidRotationMatrixException {
        return mBodyToEcefCoordinateTransformationMatrix != null ?
                new CoordinateTransformation(mBodyToEcefCoordinateTransformationMatrix,
                        FrameType.BODY_FRAME, FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME) : null;
    }

    /**
     * Gets body to ECEF coordinate transformation.
     *
     * @param threshold threshold to determine whether current body to ECEF transformation
     *                  matrix is valid or not (to check that matrix is 3x3 orthonormal).
     * @return body to ECEF coordinate transformation.
     * @throws InvalidRotationMatrixException if current body to ECEF transformation matrix
     *                                        is considered not valid (is not a 3x3 orthonormal matrix) with provided threshold.
     */
    public CoordinateTransformation getC(final double threshold) throws InvalidRotationMatrixException {
        return mBodyToEcefCoordinateTransformationMatrix != null ?
                new CoordinateTransformation(mBodyToEcefCoordinateTransformationMatrix,
                        FrameType.BODY_FRAME, FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME, threshold) : null;
    }

    /**
     * Gets body to ECEF coordinate transformation.
     *
     * @param result instance where body to ECEF coordinate transformation will be stored.
     * @return true if result instance was updated, false otherwise.
     * @throws InvalidRotationMatrixException if current body to ECEF transformation matrix
     *                                        is not valid (is not a 3x3 orthonormal matrix).
     */
    public boolean getC(final CoordinateTransformation result) throws InvalidRotationMatrixException {
        if (mBodyToEcefCoordinateTransformationMatrix != null) {
            result.setSourceType(FrameType.BODY_FRAME);
            result.setDestinationType(FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME);
            result.setMatrix(mBodyToEcefCoordinateTransformationMatrix);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets body to ECEF coordinate transformation.
     *
     * @param result    instance where body to ECEF coordinate transformation will be stored.
     * @param threshold threshold to determine whether current body to ECEF transformation
     *                  matrix is valid or not (to check that matrix is 3x3 orthonormal).
     * @return true if result instance was updated, false otherwise.
     * @throws InvalidRotationMatrixException if current body to ECEF transformation matrix
     *                                        is not valid (is not a 3x3 orthonormal matrix) with provided threshold.
     */
    public boolean getC(final CoordinateTransformation result,
                        final double threshold) throws InvalidRotationMatrixException {
        if (mBodyToEcefCoordinateTransformationMatrix != null) {
            result.setSourceType(FrameType.BODY_FRAME);
            result.setDestinationType(FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME);
            result.setMatrix(mBodyToEcefCoordinateTransformationMatrix, threshold);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets body to ECEF coordinate transformation.
     *
     * @param c body to ECEF coordinate transformation to be set.
     * @throws IllegalArgumentException if provided coordinate transformation is
     *                                  not null and is not a body to ECEF transformation.
     */
    public void setC(final CoordinateTransformation c) {
        if (c == null) {
            mBodyToEcefCoordinateTransformationMatrix = null;

        } else {

            if (c.getSourceType() != FrameType.BODY_FRAME ||
                    c.getDestinationType() != FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME) {
                throw new IllegalArgumentException();
            }

            if (mBodyToEcefCoordinateTransformationMatrix != null) {
                c.getMatrix(mBodyToEcefCoordinateTransformationMatrix);
            } else {
                mBodyToEcefCoordinateTransformationMatrix = c.getMatrix();
            }
        }
    }

    /**
     * Gets estimated ECEF user velocity resolved around x axis.
     *
     * @param result instance where estimated ECEF user velocity resolved around x axis will be stored.
     */
    public void getSpeedX(final Speed result) {
        result.setValue(mVx);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets estimated ECEF user velocity resolved around x axis.
     *
     * @return estimated ECEF user velocity resolved around x axis.
     */
    public Speed getSpeedX() {
        return new Speed(mVx, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets estimated ECEF user velocity resolved around x axis.
     *
     * @param vx estimated ECEF user velocity resolved around x axis.
     */
    public void setSpeedX(final Speed vx) {
        mVx = SpeedConverter.convert(vx.getValue().doubleValue(),
                vx.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets estimated ECEF user velocity resolved around y axis.
     *
     * @param result instance where estimated ECEF user velocity resolved around y axis will be stored.
     */
    public void getSpeedY(final Speed result) {
        result.setValue(mVy);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets estimated ECEF user velocity resolved around y axis.
     *
     * @return estimated ECEF velocity resolved around y axis.
     */
    public Speed getSpeedY() {
        return new Speed(mVy, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets estimated ECEF user velocity resolved around y axis.
     *
     * @param vy estimated ECEF user velocity resolved around y axis.
     */
    public void setSpeedY(final Speed vy) {
        mVy = SpeedConverter.convert(vy.getValue().doubleValue(),
                vy.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets estimated ECEF user velocity resolved around z axis.
     *
     * @param result instance where estimated ECEF user velocity resolved around z axis will be stored.
     */
    public void getSpeedZ(final Speed result) {
        result.setValue(mVz);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets estimated ECEF user velocity resolved around z axis.
     *
     * @return estimated ECEF velocity resolved around z axis.
     */
    public Speed getSpeedZ() {
        return new Speed(mVz, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets estimated ECEF user velocity resolved around z axis.
     *
     * @param vz estimated ECEF velocity resolved around z axis.
     */
    public void setSpeedZ(final Speed vz) {
        mVz = SpeedConverter.convert(vz.getValue().doubleValue(),
                vz.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets estimated ECEF user velocity.
     *
     * @param vx estimated ECEF velocity resolved around x axis.
     * @param vy estimated ECEF velocity resolved around y axis.
     * @param vz estimated ECEF velocity resolved around z axis.
     */
    public void setVelocityCoordinates(
            final Speed vx, final Speed vy, final Speed vz) {
        setSpeedX(vx);
        setSpeedY(vy);
        setSpeedZ(vz);
    }

    /**
     * Gets estimated ECEF user velocity.
     *
     * @param result instance where estimated ECEF user velocity will be stored.
     */
    public void getEcefVelocity(final ECEFVelocity result) {
        result.setCoordinates(mVx, mVy, mVz);
    }

    /**
     * Gets estimated ECEF user velocity.
     *
     * @return estimated ECEF user velocity.
     */
    public ECEFVelocity getEcefVelocity() {
        return new ECEFVelocity(mVx, mVy, mVz);
    }

    /**
     * Sets estimated ECEF user velocity.
     *
     * @param ecefVelocity estimated ECEF user velocity.
     */
    public void setEcefVelocity(final ECEFVelocity ecefVelocity) {
        mVx = ecefVelocity.getVx();
        mVy = ecefVelocity.getVy();
        mVz = ecefVelocity.getVz();
    }

    /**
     * Gets x coordinate of estimated ECEF user position.
     *
     * @param result instance where x coordinate of estimated ECEF user position
     *               will be stored.
     */
    public void getDistanceX(final Distance result) {
        result.setValue(mX);
        result.setUnit(DistanceUnit.METER);
    }

    /**
     * Gets x coordinate of estimated ECEF user position.
     *
     * @return x coordinate of estimated ECEF user position.
     */
    public Distance getDistanceX() {
        return new Distance(mX, DistanceUnit.METER);
    }

    /**
     * Sets x coordinate of estimated ECEF user position.
     *
     * @param x x coordinate of estimated ECEF user position.
     */
    public void setDistanceX(final Distance x) {
        mX = DistanceConverter.convert(x.getValue().doubleValue(),
                x.getUnit(), DistanceUnit.METER);
    }

    /**
     * Gets y coordinate of estimated ECEF user position.
     *
     * @param result instance where y coordinate of estimated ECEF user position
     *               will be stored.
     */
    public void getDistanceY(final Distance result) {
        result.setValue(mY);
        result.setUnit(DistanceUnit.METER);
    }

    /**
     * Gets y coordinate of estimated ECEF user position.
     *
     * @return y coordinate of estimated ECEF user position.
     */
    public Distance getDistanceY() {
        return new Distance(mY, DistanceUnit.METER);
    }

    /**
     * Sets y coordinate of estimated ECEF user position.
     *
     * @param y y coordinate of estimated ECEF user position.
     */
    public void setDistanceY(final Distance y) {
        mY = DistanceConverter.convert(y.getValue().doubleValue(),
                y.getUnit(), DistanceUnit.METER);
    }

    /**
     * Gets z coordinate of estimated ECEF user position.
     *
     * @param result instance where z coordinate of estimated ECEF user position
     *               will be stored.
     */
    public void getDistanceZ(final Distance result) {
        result.setValue(mZ);
        result.setUnit(DistanceUnit.METER);
    }

    /**
     * Gets z coordinate of estimated ECEF user position.
     *
     * @return z coordinate of estimated ECEF user position.
     */
    public Distance getDistanceZ() {
        return new Distance(mZ, DistanceUnit.METER);
    }

    /**
     * Sets z coordinate of estimated ECEF user position.
     *
     * @param z z coordinate of estimated ECEF user position.
     */
    public void setDistanceZ(final Distance z) {
        mZ = DistanceConverter.convert(z.getValue().doubleValue(),
                z.getUnit(), DistanceUnit.METER);
    }

    /**
     * Sets coordinates of estimated ECEF user position.
     *
     * @param x x coordinate of estimated ECEF user position.
     * @param y y coordinate of estimated ECEF user position.
     * @param z z coordinate of estimated ECEF user position.
     */
    public void setPositionCoordinates(final Distance x, final Distance y, final Distance z) {
        setDistanceX(x);
        setDistanceY(y);
        setDistanceZ(z);
    }

    /**
     * Gets estimated ECEF user position expressed in meters (m).
     *
     * @param result instance where estimated ECEF user position expressed
     *               in meters (m) will be stored.
     */
    public void getPosition(final Point3D result) {
        result.setInhomogeneousCoordinates(mX, mY, mZ);
    }

    /**
     * Gets estimated ECEF user position expressed in meters (m).
     *
     * @return estimated ECEF user position expressed in meters (m).
     */
    public Point3D getPosition() {
        return new InhomogeneousPoint3D(mX, mY, mZ);
    }

    /**
     * Sets estimated ECEF user position expressed in meters (m).
     *
     * @param position estimated ECEF user position expressed in
     *                 meters (m).
     */
    public void setPosition(final Point3D position) {
        mX = position.getInhomX();
        mY = position.getInhomY();
        mZ = position.getInhomZ();
    }

    /**
     * Gets estimated ECEF user position.
     *
     * @param result instance where estimated ECEF user position
     *               will be stored.
     */
    public void getEcefPosition(final ECEFPosition result) {
        result.setCoordinates(mX, mY, mZ);
    }

    /**
     * Gets estimated ECEF user position.
     *
     * @return estimated ECEF user position.
     */
    public ECEFPosition getEcefPosition() {
        return new ECEFPosition(mX, mY, mZ);
    }

    /**
     * Sets estimated ECEF user position.
     *
     * @param ecefPosition estimated ECEF user position.
     */
    public void setEcefPosition(final ECEFPosition ecefPosition) {
        mX = ecefPosition.getX();
        mY = ecefPosition.getY();
        mZ = ecefPosition.getZ();
    }

    /**
     * Gets estimated ECEF user position and velocity.
     *
     * @param result instance where estimated ECEF user position and velocity
     *               will be stored.
     */
    public void getPositionAndVelocity(final ECEFPositionAndVelocity result) {
        result.setPositionCoordinates(mX, mY, mZ);
        result.setVelocityCoordinates(mVx, mVy, mVz);
    }

    /**
     * Gets estimated ECEF user position and velocity.
     *
     * @return estimated ECEF user position and velocity.
     */
    public ECEFPositionAndVelocity getPositionAndVelocity() {
        return new ECEFPositionAndVelocity(mX, mY, mZ, mVx, mVy, mVz);
    }

    /**
     * Sets estimated ECEF user position and velocity.
     *
     * @param positionAndVelocity estimated ECEF user position and velocity.
     */
    public void setPositionAndVelocity(final ECEFPositionAndVelocity positionAndVelocity) {
        mX = positionAndVelocity.getX();
        mY = positionAndVelocity.getY();
        mZ = positionAndVelocity.getZ();
        mVx = positionAndVelocity.getVx();
        mVy = positionAndVelocity.getVy();
        mVz = positionAndVelocity.getVz();
    }

    /**
     * Gets body to ECEF frame containing coordinate transformation, position and
     * velocity.
     *
     * @param result instance where body to ECEF frame will be stored.
     * @return true if result was updated, false otherwise.
     */
    public boolean getFrame(ECEFFrame result) {
File Line
com/irurueta/navigation/indoor/position/SequentialRobustMixedPositionEstimator.java 723
com/irurueta/navigation/indoor/position/SequentialRobustRangingAndRssiPositionEstimator.java 697
            final SequentialRobustMixedPositionEstimatorListener<P> listener) {
        this(sourceQualityScores, fingerprintReadingQualityScores, sources,
                fingerprint);
        mListener = listener;
    }

    /**
     * Gets robust method used for robust position estimation using ranging data.
     *
     * @return robust method used for robust position estimation using ranging data.
     */
    public RobustEstimatorMethod getRangingRobustMethod() {
        return mRangingRobustMethod;
    }

    /**
     * Sets robust method for robust position estimation using ranging data.
     *
     * @param rangingRobustMethod robust method used for robust position estimation
     *                            using ranging data.
     * @throws LockedException if this instance is locked.
     */
    public void setRangingRobustMethod(final RobustEstimatorMethod rangingRobustMethod)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        mRangingRobustMethod = rangingRobustMethod;
    }

    /**
     * Gets robust method used for coarse robust position estimation using RSSI data.
     *
     * @return robust method used for coarse robust position estimation using RSSI
     * data.
     */
    public RobustEstimatorMethod getRssiRobustMethod() {
        return mRssiRobustMethod;
    }

    /**
     * Sets robust method used for coarse robust position estimation using RSSI data.
     *
     * @param rssiRobustMethod robust method used for coarse robust position estimation
     *                         using RSSI data.
     * @throws LockedException if this instance is locked.
     */
    public void setRssiRobustMethod(final RobustEstimatorMethod rssiRobustMethod)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        mRssiRobustMethod = rssiRobustMethod;
    }

    /**
     * Indicates whether located radio source position covariance is taken into account
     * (if available) to determine distance standard deviation for ranging measurements.
     *
     * @return true to take into account radio source position covariance during ranging
     * position estimation, false otherwise.
     */
    public boolean isRangingRadioSourcePositionCovarianceUsed() {
        return mUseRangingRadioSourcePositionCovariance;
    }

    /**
     * Specifies whether located radio source position covariance is taken into account
     * (if available) to determine distance standard deviation for ranging measurements.
     *
     * @param useRangingRadioSourcePositionCovariance true to take into account radio
     *                                                source position covariance during
     *                                                ranging position estimation,
     *                                                false otherwise.
     * @throws LockedException if this instance is locked.
     */
    public void setRangingRadioSourcePositionCovarianceUsed(
            final boolean useRangingRadioSourcePositionCovariance) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        mUseRangingRadioSourcePositionCovariance =
                useRangingRadioSourcePositionCovariance;
    }

    /**
     * Indicates whether located radio source position covariance is taken into account
     * (if available) to determine distance standard deviation for RSSI measurements.
     *
     * @return true to take into account radio source position covariance during RSSI
     * position estimation, false otherwise.
     */
    public boolean isRssiRadioSourcePositionCovarianceUsed() {
        return mUseRssiRadioSourcePositionCovariance;
    }

    /**
     * Specifies whether located radio source position covariance is taken into account
     * (if available) to determine distance standard deviation for RSSI measurements.
     *
     * @param useRssiRadioSourcePositionCovariance true to take into account radio
     *                                             source position covariance during
     *                                             RSSI position estimation, false
     *                                             otherwise.
     * @throws LockedException if this instance is locked.
     */
    public void setRssiRadioSourcePositionCovarianceUsed(
            final boolean useRssiRadioSourcePositionCovariance) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        mUseRssiRadioSourcePositionCovariance = useRssiRadioSourcePositionCovariance;
    }

    /**
     * Indicates whether ranging readings are evenly distributed among radio sources
     * taking into account quality scores of both radio sources and ranging readings.
     *
     * @return true if ranging readings are evenly distributed among radio sources,
     * false otherwise.
     */
    public boolean isRangingReadingsEvenlyDistributed() {
        return mEvenlyDistributeRangingReadings;
    }

    /**
     * Specifies whether ranging readings are evenly distributed among radio sources
     * taking into account quality scores of both radio sources and ranging readings.
     *
     * @param evenlyDistributeRangingReadings true if ranging readings are evenly
     *                                        distributed among radio sources, false
     *                                        otherwise.
     * @throws LockedException if this instance is locked.
     */
    public void setRangingReadingsEvenlyDistributed(
            final boolean evenlyDistributeRangingReadings) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        mEvenlyDistributeRangingReadings = evenlyDistributeRangingReadings;
    }

    /**
     * Gets distance standard deviation fallback value to use when none can be
     * determined from provided RSSI measurements.
     *
     * @return distance standard deviation fallback value to use when none can be
     * determined from provided RSSI measurements.
     */
    public double getRssiFallbackDistanceStandardDeviation() {
        return mRssiFallbackDistanceStandardDeviation;
    }

    /**
     * Sets distance standard deviation fallback value to use when none can be
     * determined from provided RSSI measurements.
     *
     * @param rssiFallbackDistanceStandardDeviation distance standard deviation fallback
     *                                              value to use when none can be
     *                                              determined from provided RSSI
     *                                              measurements.
     * @throws LockedException if this instance is locked.
     */
    public void setRssiFallbackDistanceStandardDeviation(
            final double rssiFallbackDistanceStandardDeviation) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mRssiFallbackDistanceStandardDeviation = rssiFallbackDistanceStandardDeviation;
    }

    /**
     * Gets distance standard deviation fallback value to use when none can be
     * determined from provided ranging measurements.
     *
     * @return distance standard deviation fallback value to use when none can be
     * determined from provided ranging measurements.
     */
    public double getRangingFallbackDistanceStandardDeviation() {
        return mRangingFallbackDistanceStandardDeviation;
    }

    /**
     * Sets distance standard deviation fallback value to use when none can be
     * determined from provided ranging measurements.
     *
     * @param rangingFallbackDistanceStandardDeviation distance standard deviation
     *                                                 fallback value to use when none
     *                                                 can be determined from provided
     *                                                 ranging measurements.
     * @throws LockedException if this instance is locked.
     */
    public void setRangingFallbackDistanceStandardDeviation(
            final double rangingFallbackDistanceStandardDeviation) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mRangingFallbackDistanceStandardDeviation =
                rangingFallbackDistanceStandardDeviation;
    }

    /**
     * Indicates whether RSSI readings are evenly distributed among radio sources
     * taking into account quality scores of both radio sources and RSSI readings.
     *
     * @return true if RSSI readings are evenly distributed among radio sources,
     * false otherwise.
     */
    public boolean isRssiReadingsEvenlyDistributed() {
        return mEvenlyDistributeRssiReadings;
    }

    /**
     * Specifies whether RSSI readings are evenly distributed among radio sources
     * taking into account quality scores of both radio sources and RSSI readings.
     *
     * @param evenlyDistributeRssiReadings true if RSSI readings are evenly distributed
     *                                     among radio sources, false otherwise.
     * @throws LockedException if this instance is locked.
     */
    public void setRssiReadingsEvenlyDistributed(
            final boolean evenlyDistributeRssiReadings) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        mEvenlyDistributeRssiReadings = evenlyDistributeRssiReadings;
    }

    /**
     * Gets amount of progress variation before notifying a progress change during
     * estimation.
     *
     * @return amount of progress variation before notifying a progress change during
     * estimation.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * estimation.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during estimation.
     * @throws IllegalArgumentException if progress delta is less than zero or
     *                                  greater than 1.
     * @throws LockedException          if this instance is locked.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0 (which
     * is equivalent to 100%) for robust position estimation on ranging data. The
     * amount of confidence indicates the probability that the estimated result is
     * correct. Usually this value will be close to 1.0, but not exactly 1.0.
     *
     * @return amount of confidence for robust position estimation as a value between
     * 0.0 and 1.0.
     */
    public double getRangingConfidence() {
        return mRangingConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%) for robust position estimation on ranging data. The amount
     * of confidence indicates the probability that the estimated result is correct.
     * Usually this value will be close to 1.0, but not exactly 1.0.
     *
     * @param rangingConfidence confidence to be set for robust position estimation as
     *                          a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if estimator is locked.
     */
    public void setRangingConfidence(final double rangingConfidence) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rangingConfidence < MIN_CONFIDENCE || rangingConfidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mRangingConfidence = rangingConfidence;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%) for robust position estimation on RSSI data. The amount of
     * confidence indicates the probability that the estimated result is correct.
     * Usually this value will be close to 1.0, but not exactly 1.0.
     *
     * @return amount of confidence for robust position estimation as a value between
     * 0.0 and 1.0.
     */
    public double getRssiConfidence() {
        return mRssiConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%) for robust position estimation on RSSI data. The amount
     * of confidence indicates the probability that the estimated result is correct.
     * Usually this value will be close to 1.0, but not exactly 1.0.
     *
     * @param rssiConfidence amount of confidence for robust position estimation as
     *                       a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if estimator is locked.
     */
    public void setRssiConfidence(final double rssiConfidence) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rssiConfidence < MIN_CONFIDENCE || rssiConfidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mRssiConfidence = rssiConfidence;
    }

    /**
     * Gets maximum allowed number of iterations for robust ranging position estimation.
     * When the maximum number of iterations is exceeded, an approximate result might
     * be available for retrieval.
     *
     * @return maximum allowed number of iterations for position estimation.
     */
    public int getRangingMaxIterations() {
        return mRangingMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations for robust ranging position
     * estimation.
     * When the maximum number of iterations is exceeded, an approximate result might
     * be available for retrieval.
     *
     * @param rangingMaxIterations maximum allowed number of iterations to be set for
     *                             position estimation.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if estimator is locked.
     */
    public void setRangingMaxIterations(final int rangingMaxIterations)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rangingMaxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mRangingMaxIterations = rangingMaxIterations;
    }

    /**
     * Gets maximum allowed number of iterations for robust RSSI position estimation.
     * When the maximum number of iterations is exceeded, an approximate result might
     * be available for retrieval.
     *
     * @return maximum allowed number of iterations for position estimation.
     */
    public int getRssiMaxIterations() {
        return mRssiMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations for robust RSSI position estimation.
     * When the maximum number of iterations is exceeded, an approximate result might
     * be available for retrieval.
     *
     * @param rssiMaxIterations maximum allowed number of iterations to be set for
     *                          position estimation.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if estimator is locked.
     */
    public void setRssiMaxIterations(final int rssiMaxIterations) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rssiMaxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mRssiMaxIterations = rssiMaxIterations;
    }

    /**
     * Indicates whether result is refined using all found inliers.
     *
     * @return true if result is refined, false otherwise.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result is refined using all found inliers.
     *
     * @param refineResult true if result is refined, false otherwise.
     * @throws LockedException if this instance is locked.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Indicates whether a linear solver is used for preliminary solution estimation
     * using ranging measurements.
     * The result obtained on each preliminary solution might be later refined.
     *
     * @return true if a linear solver is used for preliminary solution estimation on
     * ranging readings.
     */
    public boolean isRangingLinearSolverUsed() {
        return mUseRangingLinearSolver;
    }

    /**
     * Specifies whether a linear solver is used for preliminary solution estimation
     * using ranging measurements.
     * The result obtained on each preliminary solution might be later refined.
     *
     * @param useRangingLinearSolver true if a linear solver is used for preliminary
     *                               solution estimation on ranging readings.
     * @throws LockedException if estimator is locked.
     */
    public void setRangingLinearSolverUsed(final boolean useRangingLinearSolver)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mUseRangingLinearSolver = useRangingLinearSolver;
    }

    /**
     * Indicates whether a linear solver is used for preliminary solution estimation
     * using RSSI measurements.
     * The result obtained on each preliminary solution might be later refined.
     *
     * @return true if a linear solver is used for preliminary solution estimation on
     * RSSI readings.
     */
    public boolean isRssiLinearSolverUsed() {
        return mUseRssiLinearSolver;
    }

    /**
     * Specifies whether a linear solver is used for preliminary solution estimation
     * using RSSI measurements.
     * The result obtained on each preliminary solution might be later refined.
     *
     * @param useRssiLinearSolver true if a linear solver is used for preliminary
     *                            solution estimation on RSSI readings.
     * @throws LockedException if estimator is locked.
     */
    public void setRssiLinearSolverUsed(final boolean useRssiLinearSolver)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mUseRssiLinearSolver = useRssiLinearSolver;
    }

    /**
     * Indicates whether an homogeneous linear solver is used either to estimate
     * preliminary solutions or an initial solution for preliminary solutions that
     * will be later refined on the ranging fine estimation.
     *
     * @return true to use an homogeneous linear solver for preliminary solutions
     * during ranging fine position estimation.
     */
    public boolean isRangingHomogeneousLinearSolverUsed() {
        return mUseRangingHomogeneousLinearSolver;
    }

    /**
     * Specifies whether an homogeneous linear solver is used either to estimate
     * preliminary solutions or an initial solution for preliminary solutions that
     * will be later refined on the ranging fine estimation.
     *
     * @param useRangingHomogeneousLinearSolver true to use an homogeneous linear
     *                                          solver for preliminary solutions during
     *                                          ranging fine position estimation.
     * @throws LockedException if estimator is locked.
     */
    public void setRangingHomogeneousLinearSolverUsed(
            final boolean useRangingHomogeneousLinearSolver) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mUseRangingHomogeneousLinearSolver = useRangingHomogeneousLinearSolver;
    }

    /**
     * Indicates whether an homogeneous linear solver is used either to estimate
     * preliminary solutions or an initial solution for preliminary solutions that
     * will be later refined on the RSSI coarse estimation.
     *
     * @return true to use an homogeneous linear solver for preliminary solutions
     * during RSSI coarse position estimation.
     */
    public boolean isRssiHomogeneousLinearSolverUsed() {
        return mUseRssiHomogeneousLinearSolver;
    }

    /**
     * Specifies whether an homogeneous linear solver is used either to estimate
     * preliminary solutions or an initial solution for preliminary solutions that
     * will be later refined on the RSSI coarse estimation.
     *
     * @param useRssiHomogeneousLinearSolver true to use an homogeneous linear
     *                                       solver for preliminary solutions during
     *                                       RSSI fine position estimation.
     * @throws LockedException if estimator is locked.
     */
    public void setRssiHomogeneousLinearSolverUsed(
            final boolean useRssiHomogeneousLinearSolver) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mUseRssiHomogeneousLinearSolver = useRssiHomogeneousLinearSolver;
    }

    /**
     * Indicates whether preliminary ranging solutions are refined after an initial
     * linear solution is found.
     * If no initial preliminary solution is found using a linear solver, a non
     * linear solver will be used regardless of this value using an average solution
     * as the initial value to be refined.
     *
     * @return true if preliminary ranging solutions must be refined after an initial
     * linear solution, false otherwise.
     */
    public boolean isRangingPreliminarySolutionRefined() {
        return mRefineRangingPreliminarySolutions;
    }

    /**
     * Specifies whether preliminary ranging solutions are refined after an initial
     * linear solution is found.
     * If no initial preliminary solution is found using a linear solver, a non
     * linear solver will be used regardless of this value using an average solution
     * as the initial value to be refined.
     *
     * @param refineRangingPreliminarySolutions true if preliminary ranging solutions
     *                                          must be refined after an initial linear
     *                                          solution, false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setRangingPreliminarySolutionRefined(
            final boolean refineRangingPreliminarySolutions) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mRefineRangingPreliminarySolutions = refineRangingPreliminarySolutions;
    }

    /**
     * Indicates whether preliminary RSSI solutions are refined after an initial
     * linear solution is found.
     * If no initial preliminary solution is found using a linear solver, a non
     * linear solver will be used regardless of this value using an average solution
     * as the initial value to be refined.
     *
     * @return true if preliminary RSSI solutions must be refined after an initial
     * linear solution, false otherwise.
     */
    public boolean isRssiPreliminarySolutionRefined() {
        return mRefineRssiPreliminarySolutions;
    }

    /**
     * Specifies whether preliminary RSSI solutions are refined after an initial
     * linear solution is found.
     * If no initial preliminary solution is found using a linear solver, a non
     * linear solver will be used regardless of this value using an average solution
     * as the initial value ot be refined.
     *
     * @param refineRssiPreliminarySolutions true if preliminary RSSI solutions must
     *                                       be refined after an initial linear
     *                                       solution, false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setRssiPreliminarySolutionRefined(
            final boolean refineRssiPreliminarySolutions) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mRefineRssiPreliminarySolutions = refineRssiPreliminarySolutions;
    }

    /**
     * Gets size of subsets to be checked during ranging robust estimation.
     *
     * @return size of subsets to be checked during ranging robust estimation.
     */
    public int getRangingPreliminarySubsetSize() {
        return mRangingPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during ranging robust estimation.
     *
     * @param rangingPreliminarySubsetSize size of subsets to be checked during
     *                                     ranging robust estimation.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is less than {@link #getMinRequiredSources()}.
     */
    public void setRangingPreliminarySubsetSize(final int rangingPreliminarySubsetSize)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rangingPreliminarySubsetSize < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mRangingPreliminarySubsetSize = rangingPreliminarySubsetSize;
    }

    /**
     * Gets size of subsets to be checked during RSSI robust estimation.
     *
     * @return size of subsets to be checked during RSSI robust estimation.
     */
    public int getRssiPreliminarySubsetSize() {
        return mRssiPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during RSSI robust estimation.
     *
     * @param rssiPreliminarySubsetSize size of subsets to be checked during
     *                                  RSSI robust estimation.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is less than {@link #getMinRequiredSources()}.
     */
    public void setRssiPreliminarySubsetSize(final int rssiPreliminarySubsetSize)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rssiPreliminarySubsetSize < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mRssiPreliminarySubsetSize = rssiPreliminarySubsetSize;
    }

    /**
     * Gets threshold to determine when samples are inliers or not, used during robust
     * fine ranging position estimation.
     * If not defined, default threshold will be used.
     *
     * @return threshold for ranging estimation or null.
     */
    public Double getRangingThreshold() {
        return mRangingThreshold;
    }

    /**
     * Sets threshold to determine when samples are inliers or not, used during robust
     * fine ranging position estimation.
     * If not defined, default threshold will be used.
     *
     * @param rangingThreshold threshold for ranging estimation or null.
     * @throws LockedException if estimator is locked.
     */
    public void setRangingThreshold(final Double rangingThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mRangingThreshold = rangingThreshold;
    }

    /**
     * Gets threshold to determine when samples are inliers or not, used during robust
     * coarse RSSI position estimation.
     * If not defined, default threshold will be used.
     *
     * @return threshold for RSSI estimation or null.
     */
    public Double getRssiThreshold() {
        return mRssiThreshold;
    }

    /**
     * Sets threshold to determine when samples are inliers or not, used during robust
     * coarse RSSI position estimation.
     * If not defined, default threshold will be used.
     *
     * @param rssiThreshold threshold for RSSI estimation or null.
     * @throws LockedException if estimator is locked.
     */
    public void setRssiThreshold(final Double rssiThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mRssiThreshold = rssiThreshold;
    }

    /**
     * Gets located radio sources used for lateration.
     *
     * @return located radio sources used for lateration.
     */
    public List<? extends RadioSourceLocated<P>> getSources() {
        return mSources;
    }

    /**
     * Sets located radio sources used for lateration.
     *
     * @param sources located radio sources used for lateration.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is null or the number of
     *                                  provided sources is less than the required
     *                                  minimum.
     */
    public void setSources(final List<? extends RadioSourceLocated<P>> sources)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        internalSetSources(sources);
    }

    /**
     * Gets fingerprint containing readings at an unknown location for provided located
     * radio sources.
     *
     * @return fingerprint containing readings at an unknown location for provided
     * located radio sources.
     */
    public Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> getFingerprint() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2784
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4195
                && mSequences.size() >= getMinimumRequiredSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public abstract void calibrate() throws LockedException, NotReadyException,
            CalibrationException;

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasX() {
        return mEstimatedBiases != null ? mEstimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasY() {
        return mEstimatedBiases != null ? mEstimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasZ() {
        return mEstimatedBiases != null ? mEstimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[0],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[1],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[2],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredSequences()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredSequences}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than
     *                                  {@link #getMinimumRequiredSequences}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredSequences()) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 690
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2701
            final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Gets known x coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return x coordinate of gyroscope bias.
     */
    @Override
    public double getBiasX() {
        return mBiasX;
    }

    /**
     * Sets known x coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @param biasX x coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
    }

    /**
     * Gets known y coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return y coordinate of gyroscope bias.
     */
    @Override
    public double getBiasY() {
        return mBiasY;
    }

    /**
     * Sets known y coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @param biasY y coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = biasY;
    }

    /**
     * Gets known z coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return z coordinate of gyroscope bias.
     */
    @Override
    public double getBiasZ() {
        return mBiasZ;
    }

    /**
     * Sets known z coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = biasZ;
    }

    /**
     * Gets known x coordinate of gyroscope bias.
     *
     * @return x coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedX() {
        return new AngularSpeed(mBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known x coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(mBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known x coordinate of gyroscope bias.
     *
     * @param biasX x coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final AngularSpeed biasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasX = convertAngularSpeed(biasX);
    }

    /**
     * Gets known y coordinate of gyroscope bias.
     *
     * @return y coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedY() {
        return new AngularSpeed(mBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known y coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(mBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known y coordinate of gyroscope bias.
     *
     * @param biasY y coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final AngularSpeed biasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasY = convertAngularSpeed(biasY);
    }

    /**
     * Gets known z coordinate of gyroscope bias.
     *
     * @return z coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedZ() {
        return new AngularSpeed(mBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known z coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(mBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known z coordinate of gyroscope bias.
     *
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Sets known gyroscope bias coordinates expressed in radians per second
     * (rad/s).
     *
     * @param biasX x coordinate of gyroscope bias.
     * @param biasY y coordinate of gyroscope bias.
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(
            final double biasX, final double biasY, final double biasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasX = biasX;
        mBiasY = biasY;
        mBiasZ = biasZ;
    }

    /**
     * Sets known gyroscope bias coordinates.
     *
     * @param biasX x coordinate of gyroscope bias.
     * @param biasY y coordinate of gyroscope bias.
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(
            final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasX = convertAngularSpeed(biasX);
        mBiasY = convertAngularSpeed(biasY);
        mBiasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Gets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinate of known bias.
     */
    @Override
    public double[] getBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mBiasX;
        result[1] = mBiasY;
        result[2] = mBiasZ;
    }

    /**
     * Sets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param bias known gyroscope bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mBiasX = bias[0];
        mBiasY = bias[1];
        mBiasZ = bias[2];
    }

    /**
     * Gets known gyroscope bias as a column matrix.
     *
     * @return known gyroscope bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known gyroscope bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mBiasX);
        result.setElementAtIndex(1, mBiasY);
        result.setElementAtIndex(2, mBiasZ);
    }

    /**
     * Sets known gyroscope bias as a column matrix.
     *
     * @param bias gyroscope bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS
                || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mBiasX = bias.getElementAtIndex(0);
        mBiasY = bias.getElementAtIndex(1);
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Indicates whether calibrator is ready to start the calibration.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null
                && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3701
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3764
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @return constant rotation rate of turntable.
     */
    public double getTurntableRotationRate() {
        return mTurntableRotationRate;
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final double turntableRotationRate) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (turntableRotationRate <= 0.0) {
            throw new IllegalArgumentException();
        }

        mTurntableRotationRate = turntableRotationRate;
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @return constant rotation rate of turntable.
     */
    public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
        return new AngularSpeed(mTurntableRotationRate,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @param result instance where result will be stored.
     */
    public void getTurntableRotationRateAsAngularSpeed(
            final AngularSpeed result) {
        result.setValue(mTurntableRotationRate);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final AngularSpeed turntableRotationRate)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setTurntableRotationRate(
                convertAngularSpeed(turntableRotationRate));
    }

    /**
     * Gets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @return time interval between measurements.
     */
    public double getTimeInterval() {
        return mTimeInterval;
    }

    /**
     * Sets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTimeInterval(final double timeInterval)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (timeInterval <= 0.0) {
            throw new IllegalArgumentException();
        }
        mTimeInterval = timeInterval;
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @return time interval between measurements.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(mTimeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @param result instance where result will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(mTimeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between measurements being captured.
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTimeInterval(final Time timeInterval)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setTimeInterval(convertTime(timeInterval));
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @param measurements collection of body kinematics measurements at a
     *                     known position witn unknown orientations.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMeasurements(
            final Collection<StandardDeviationBodyKinematics> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return mPosition;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final NEDPosition result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (mPosition != null) {
            final NEDVelocity velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    mPosition.getX(), mPosition.getY(), mPosition.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Indicates whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @return true if G-dependent cross biases will be estimated,
     * false otherwise.
     */
    public boolean isGDependentCrossBiasesEstimated() {
        return mEstimateGDependentCrossBiases;
    }

    /**
     * Specifies whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setGDependentCrossBiasesEstimated(
            final boolean estimateGDependentCrossBiases)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mEstimateGDependentCrossBiases = estimateGDependentCrossBiases;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public KnownBiasTurntableGyroscopeCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3659
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3719
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @return constant rotation rate of turntable.
     */
    public double getTurntableRotationRate() {
        return mTurntableRotationRate;
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final double turntableRotationRate) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (turntableRotationRate <= 0.0) {
            throw new IllegalArgumentException();
        }

        mTurntableRotationRate = turntableRotationRate;
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @return constant rotation rate of turntable.
     */
    public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
        return new AngularSpeed(mTurntableRotationRate,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @param result instance where result will be stored.
     */
    public void getTurntableRotationRateAsAngularSpeed(
            final AngularSpeed result) {
        result.setValue(mTurntableRotationRate);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final AngularSpeed turntableRotationRate)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setTurntableRotationRate(
                convertAngularSpeed(turntableRotationRate));
    }

    /**
     * Gets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @return time interval between measurements.
     */
    public double getTimeInterval() {
        return mTimeInterval;
    }

    /**
     * Sets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTimeInterval(final double timeInterval)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (timeInterval <= 0.0) {
            throw new IllegalArgumentException();
        }
        mTimeInterval = timeInterval;
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @return time interval between measurements.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(mTimeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @param result instance where result will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(mTimeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between measurements being captured.
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTimeInterval(final Time timeInterval)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setTimeInterval(convertTime(timeInterval));
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    public List<StandardDeviationBodyKinematics> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @param measurements collection of body kinematics measurements at a
     *                     known position witn unknown orientations.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMeasurements(
            final List<StandardDeviationBodyKinematics> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return mPosition;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final NEDPosition result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (mPosition != null) {
            final NEDVelocity velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    mPosition.getX(), mPosition.getY(), mPosition.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCommonAxisUsed(final boolean commonAxisUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Indicates whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @return true if G-dependent cross biases will be estimated,
     * false otherwise.
     */
    public boolean isGDependentCrossBiasesEstimated() {
        return mEstimateGDependentCrossBiases;
    }

    /**
     * Specifies whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setGDependentCrossBiasesEstimated(
            final boolean estimateGDependentCrossBiases)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mEstimateGDependentCrossBiases = estimateGDependentCrossBiases;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public RobustKnownBiasTurntableGyroscopeCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/AccelerationFixer.java 84
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 109
            mCrossCouplingErrors = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
        } catch (final WrongSizeException ignore) {
            // never happens
        }
    }

    /**
     * Gets bias values expressed in meters per squared second (m/s^2).
     *
     * @return bias values expressed in meters per squared second.
     */
    public Matrix getBias() {
        return new Matrix(mBias);
    }

    /**
     * Gets bias values expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result will be stored.
     */
    public void getBias(final Matrix result) {
        mBias.copyTo(result);
    }

    /**
     * Sets bias values expressed in meters per squared second (m/s^2).
     *
     * @param bias bias values expressed in meters per squared second.
     *             Must be 3x1.
     */
    public void setBias(final Matrix bias) {
        if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        mBias = bias;
    }

    /**
     * Gets bias values expressed in meters per squared second (m/s^2).
     *
     * @return bias values expressed in meters per squared second.
     */
    public double[] getBiasArray() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getBiasArray(result);
        return result;
    }

    /**
     * Gets bias values expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getBiasArray(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        try {
            mBias.toArray(result);
        } catch (final WrongSizeException ignore) {
            // never happens
        }
    }

    /**
     * Sets bias values expressed in meters per squared second (m/s^2).
     *
     * @param bias bias values expressed in meters per squared second (m/s^2).
     *             Must have length 3.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void setBias(final double[] bias) {
        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        try {
            mBias.fromArray(bias);
        } catch (final WrongSizeException ignore) {
            // never happens
        }
    }

    /**
     * Gets x-coordinate of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @return x-coordinate of bias expressed in meters per squared
     * second (m/s^2).
     */
    public double getBiasX() {
        return mBias.getElementAtIndex(0);
    }

    /**
     * Sets x-coordinate of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @param biasX x-coordinate of bias expressed in meters per squared
     *              second (m/s^2).
     */
    public void setBiasX(final double biasX) {
        mBias.setElementAtIndex(0, biasX);
    }

    /**
     * Gets y-coordinate of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @return y-coordinate of bias expressed in meters per squared
     * second (m/s^2).
     */
    public double getBiasY() {
        return mBias.getElementAtIndex(1);
    }

    /**
     * Sets y-coordinate of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @param biasY y-coordinate of bias expressed in meters per squared
     *              second (m/s^2).
     */
    public void setBiasY(final double biasY) {
        mBias.setElementAtIndex(1, biasY);
    }

    /**
     * Gets z-coordinate of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @return z-coordinate of bias expressed in meters per squared
     * second (m/s^2).
     */
    public double getBiasZ() {
        return mBias.getElementAtIndex(2);
    }

    /**
     * Sets z-coordinate of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @param biasZ z-coordinate of bias expressed in meters per squared
     *              second (m/s^2).
     */
    public void setBiasZ(final double biasZ) {
        mBias.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets coordinates of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @param biasX x-coordinate of bias.
     * @param biasY y-coordinate of bias.
     * @param biasZ z-coordinate of bias.
     */
    public void setBias(
            final double biasX, final double biasY, final double biasZ) {
        setBiasX(biasX);
        setBiasY(biasY);
        setBiasZ(biasZ);
    }

    /**
     * Gets cross coupling errors matrix.
     *
     * @return cross coupling errors matrix.
     */
    public Matrix getCrossCouplingErrors() {
        return new Matrix(mCrossCouplingErrors);
    }

    /**
     * Gets cross coupling errors matrix.
     *
     * @param result instance where result will be stored.
     */
    public void getCrossCouplingErrors(final Matrix result) {
        mCrossCouplingErrors.copyTo(result);
    }

    /**
     * Sets cross coupling errors matrix.
     *
     * @param crossCouplingErrors cross coupling errors matrix. Must be 3x3.
     * @throws AlgebraException         if provided matrix cannot be inverted.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setCrossCouplingErrors(final Matrix crossCouplingErrors)
            throws AlgebraException {
        if (crossCouplingErrors.getRows() != BodyKinematics.COMPONENTS
                || crossCouplingErrors.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mCrossCouplingErrors = crossCouplingErrors;

        mIdentity.add(crossCouplingErrors, mTmp1);

        Utils.inverse(mTmp1, mTmp2);
    }

    /**
     * Gets x scaling factor.
     *
     * @return x scaling factor.
     */
    public double getSx() {
        return mCrossCouplingErrors.getElementAt(0, 0);
    }

    /**
     * Sets x scaling factor
     *
     * @param sx x scaling factor.
     */
    public void setSx(final double sx) {
        mCrossCouplingErrors.setElementAt(0, 0, sx);
    }

    /**
     * Gets y scaling factor.
     *
     * @return y scaling factor.
     */
    public double getSy() {
        return mCrossCouplingErrors.getElementAt(1, 1);
    }

    /**
     * Sets y scaling factor.
     *
     * @param sy y scaling factor.
     */
    public void setSy(final double sy) {
        mCrossCouplingErrors.setElementAt(1, 1, sy);
    }

    /**
     * Gets z scaling factor.
     *
     * @return z scaling factor.
     */
    public double getSz() {
        return mCrossCouplingErrors.getElementAt(2, 2);
    }

    /**
     * Sets z scaling factor.
     *
     * @param sz z scaling factor.
     */
    public void setSz(final double sz) {
        mCrossCouplingErrors.setElementAt(2, 2, sz);
    }

    /**
     * Gets x-y cross coupling error.
     *
     * @return x-y cross coupling error.
     */
    public double getMxy() {
        return mCrossCouplingErrors.getElementAt(0, 1);
    }

    /**
     * Sets x-y cross coupling error.
     *
     * @param mxy x-y cross coupling error.
     */
    public void setMxy(final double mxy) {
        mCrossCouplingErrors.setElementAt(0, 1, mxy);
    }

    /**
     * Gets x-z cross coupling error.
     *
     * @return x-z cross coupling error.
     */
    public double getMxz() {
        return mCrossCouplingErrors.getElementAt(0, 2);
    }

    /**
     * Sets x-z cross coupling error.
     *
     * @param mxz x-z cross coupling error.
     */
    public void setMxz(final double mxz) {
        mCrossCouplingErrors.setElementAt(0, 2, mxz);
    }

    /**
     * Gets y-x cross coupling error.
     *
     * @return y-x cross coupling error.
     */
    public double getMyx() {
        return mCrossCouplingErrors.getElementAt(1, 0);
    }

    /**
     * Sets y-x cross coupling error.
     *
     * @param myx y-x cross coupling error.
     */
    public void setMyx(final double myx) {
        mCrossCouplingErrors.setElementAt(1, 0, myx);
    }

    /**
     * Gets y-z cross coupling error.
     *
     * @return y-z cross coupling error.
     */
    public double getMyz() {
        return mCrossCouplingErrors.getElementAt(1, 2);
    }

    /**
     * Sets y-z cross coupling error.
     *
     * @param myz y-z cross coupling error.
     */
    public void setMyz(final double myz) {
        mCrossCouplingErrors.setElementAt(1, 2, myz);
    }

    /**
     * Gets z-x cross coupling error.
     *
     * @return z-x cross coupling error.
     */
    public double getMzx() {
        return mCrossCouplingErrors.getElementAt(2, 0);
    }

    /**
     * Sets z-x cross coupling error.
     *
     * @param mzx z-x cross coupling error.
     */
    public void setMzx(final double mzx) {
        mCrossCouplingErrors.setElementAt(2, 0, mzx);
    }

    /**
     * Gets z-y cross coupling error.
     *
     * @return z-y cross coupling error.
     */
    public double getMzy() {
        return mCrossCouplingErrors.getElementAt(2, 1);
    }

    /**
     * Sets z-y cross coupling error.
     *
     * @param mzy z-y cross coupling error.
     */
    public void setMzy(final double mzy) {
        mCrossCouplingErrors.setElementAt(2, 1, mzy);
    }

    /**
     * Sets scaling factors.
     *
     * @param sx x scaling factor.
     * @param sy y scaling factor.
     * @param sz z scaling factor.
     */
    public void setScalingFactors(
            final double sx, final double sy, final double sz) {
        setSx(sx);
        setSy(sy);
        setSz(sz);
    }

    /**
     * Sets cross coupling errors.
     *
     * @param mxy x-y cross coupling error.
     * @param mxz x-z cross coupling error.
     * @param myx y-x cross coupling error.
     * @param myz y-z cross coupling error.
     * @param mzx z-x cross coupling error.
     * @param mzy z-y cross coupling error.
     */
    public void setCrossCouplingErrors(
            final double mxy, final double mxz,
            final double myx, final double myz,
            final double mzx, final double mzy) {
        setMxy(mxy);
        setMxz(mxz);
        setMyx(myx);
        setMyz(myz);
        setMzx(mzx);
        setMzy(mzy);
    }

    /**
     * Sets scaling factors and cross coupling errors.
     *
     * @param sx  x scaling factor.
     * @param sy  y scaling factor.
     * @param sz  z scaling factor.
     * @param mxy x-y cross coupling error.
     * @param mxz x-z cross coupling error.
     * @param myx y-x cross coupling error.
     * @param myz y-z cross coupling error.
     * @param mzx z-x cross coupling error.
     * @param mzy z-y cross coupling error.
     */
    public void setScalingFactorsAndCrossCouplingErrors(
            final double sx, final double sy, final double sz,
            final double mxy, final double mxz,
            final double myx, final double myz,
            final double mzx, final double mzy) {
        setScalingFactors(sx, sy, sz);
        setCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
    }

    /**
     * Fixes provided measured specific force values by undoing the errors
     * introduced by the accelerometer model to restore the true specific
     * force.
     * This method uses last provided bias and cross coupling errors.
     *
     * @param measuredF measured specific force expressed in meters
     *                  per squared second (m/s^2). Must have length 3.
     * @param result    instance where restored true specific force will be
     *                  stored. Must have length 3.
     * @throws AlgebraException         if there are numerical instabilities.
     * @throws IllegalArgumentException if any of the provided parameters does
     *                                  not have proper size.
     */
    public void fix(final double[] measuredF, final double[] result)
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1343
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2019
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(
            final float progressDelta) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(
            final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(
            final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(
            final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(
            final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFx() {
        return mEstimatedBiases != null ? mEstimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFy() {
        return mEstimatedBiases != null ? mEstimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFz() {
        return mEstimatedBiases != null ? mEstimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    public Acceleration getEstimatedBiasFxAsAcceleration() {
        return mEstimatedBiases != null ?
                new Acceleration(mEstimatedBiases[0],
                        AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[0]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    public Acceleration getEstimatedBiasFyAsAcceleration() {
        return mEstimatedBiases != null ?
                new Acceleration(mEstimatedBiases[1],
                        AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[1]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    public Acceleration getEstimatedBiasFzAsAcceleration() {
        return mEstimatedBiases != null ?
                new Acceleration(mEstimatedBiases[2],
                        AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[2]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration solution.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2351
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2558
        mInitialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mInitialBiasX;
        result[1] = mInitialBiasY;
        result[2] = mInitialBiasZ;
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mInitialBiasX = initialBias[0];
        mInitialBiasY = initialBias[1];
        mInitialBiasZ = initialBias[2];
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     *
     * @return initial bias to be used to find a solution as a column matrix.
     */
    @Override
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialBiasX);
        result.setElementAtIndex(1, mInitialBiasY);
        result.setElementAtIndex(2, mInitialBiasZ);
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS
                || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mInitialBiasX = initialBias.getElementAtIndex(0);
        mInitialBiasY = initialBias.getElementAtIndex(1);
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
File Line
com/irurueta/navigation/inertial/calibration/IMUBiasEstimator.java 3340
com/irurueta/navigation/inertial/calibration/IMUNoiseEstimator.java 704
                mBiasAngularRateX, mBiasAngularRateY, mBiasAngularRateZ);
    }

    /**
     * Gets estimated variance of x coordinate of accelerometer sensed specific
     * force expressed in (m^2/s^4).
     *
     * @return estimated variance of x coordinate of sensed specific force.
     */
    public double getVarianceFx() {
        return mVarianceFx;
    }

    /**
     * Gets estimated variance of y coordinate of accelerometer sensed specific
     * force expressed in (m^2/s^4).
     *
     * @return estimated variance of y coordinate of sensed specific force.
     */
    public double getVarianceFy() {
        return mVarianceFy;
    }

    /**
     * Gets estimated variance of z coordinate of accelerometer sensed specific
     * force expressed in (m^2/s^4).
     *
     * @return estimated variance of z coordinate of sensed specific force.
     */
    public double getVarianceFz() {
        return mVarianceFz;
    }

    /**
     * Gets estimated variance of x coordinate of gyroscope sensed angular rate
     * expressed in (rad^2/s^2).
     *
     * @return estimated variance of x coordinate of sensed angular rate.
     */
    public double getVarianceAngularRateX() {
        return mVarianceAngularRateX;
    }

    /**
     * Gets estimated variance of y coordinate of gyroscope sensed angular rate
     * expressed in (rad^2/s^2).
     *
     * @return estimated variance of y coordinate of sensed angular rate.
     */
    public double getVarianceAngularRateY() {
        return mVarianceAngularRateY;
    }

    /**
     * Gets estimated variance of z coordinate of gyroscope sensed angular rate
     * expressed in (rad^2/s^2).
     *
     * @return estimated variance of z coordinate of sensed angular rate.
     */
    public double getVarianceAngularRateZ() {
        return mVarianceAngularRateZ;
    }

    /**
     * Gets estimated standard deviation of x coordinate of accelerometer
     * sensed specific force expressed in (m/s^2).
     *
     * @return estimated standard deviation of x coordinate of sensed specific force.
     */
    public double getStandardDeviationFx() {
        return Math.sqrt(mVarianceFx);
    }

    /**
     * Gets estimated standard deviation of x coordinate of accelerometer
     * sensed specific force.
     *
     * @return estimated standard deviation of x coordinate of sensed specific force.
     */
    public Acceleration getStandardDeviationFxAsAcceleration() {
        return new Acceleration(getStandardDeviationFx(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated standard deviation of x coordinate of accelerometer
     * sensed specific force.
     *
     * @param result instance where estimated standard deviation of x coordinate
     *               of sensed specific force will be stored.
     */
    public void getStandardDeviationFxAsAcceleration(final Acceleration result) {
        result.setValue(getStandardDeviationFx());
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated standard deviation of y coordinate of accelerometer
     * sensed specific force expressed in (m/s^2).
     *
     * @return estimated standard deviation of y coordinate of sensed specific
     * force.
     */
    public double getStandardDeviationFy() {
        return Math.sqrt(mVarianceFy);
    }

    /**
     * Gets estimated standard deviation of y coordinate of accelerometer
     * sensed specific force.
     *
     * @return estimated standard deviation of y coordinate of sensed specific
     * force.
     */
    public Acceleration getStandardDeviationFyAsAcceleration() {
        return new Acceleration(getStandardDeviationFy(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated standard deviation of y coordinate of accelerometer
     * sensed specific force.
     *
     * @param result instance where estimated standard deviation of y coordinate
     *               of sensed specific force will be stored.
     */
    public void getStandardDeviationFyAsAcceleration(final Acceleration result) {
        result.setValue(getStandardDeviationFy());
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated standard deviation of z coordinate of accelerometer
     * sensed specific force expressed in (m/s^2).
     *
     * @return estimated standard deviation of z coordinate of sensed specific
     * force.
     */
    public double getStandardDeviationFz() {
        return Math.sqrt(mVarianceFz);
    }

    /**
     * Gets estimated standard deviation of z coordinate of accelerometer
     * sensed specific force.
     *
     * @return estimated standard deviation of z coordinate of sensed specific
     * force.
     */
    public Acceleration getStandardDeviationFzAsAcceleration() {
        return new Acceleration(getStandardDeviationFz(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated standard deviation of z coordinate of accelerometer
     * sensed specific force.
     *
     * @param result instance where estimated standard deviation of z coordinate
     *               of sensed specific force will be stored.
     */
    public void getStandardDeviationFzAsAcceleration(final Acceleration result) {
        result.setValue(getStandardDeviationFz());
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets average of estimated standard deviation of accelerometer sensed specific
     * force for all coordinates expressed in meters per squared second (m/s^2).
     *
     * @return average of estimated standard deviation of accelerometer.
     */
    public double getAverageAccelerometerStandardDeviation() {
        return (getStandardDeviationFx() + getStandardDeviationFy()
                + getStandardDeviationFz()) / 3.0;
    }

    /**
     * Gets average of estimated standard deviation of accelerometer sensed specific
     * force for all coordinates.
     *
     * @return average of estimated standard deviation of accelerometer.
     */
    public Acceleration getAverageAccelerometerStandardDeviationAsAcceleration() {
        return new Acceleration(getAverageAccelerometerStandardDeviation(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets average of estimated standard deviation of accelerometer sensed specific
     * force for all coordinates.
     *
     * @param result instance where result data will be copied to.
     */
    public void getAverageAccelerometerStandardDeviationAsAcceleration(
            final Acceleration result) {
        result.setValue(getAverageAccelerometerStandardDeviation());
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated standard deviation of x coordinate of gyroscope sensed angular
     * rate expressed in (rad/s).
     *
     * @return estimated standard deviaton of x coordinate of sensed angular rate.
     */
    public double getStandardDeviationAngularRateX() {
        return Math.sqrt(mVarianceAngularRateX);
    }

    /**
     * Gets estimated standard deviation of x coordinate of gyroscope sensed angular
     * rate.
     *
     * @return estimated standard deviation of x coordinate of sensed angular rate.
     */
    public AngularSpeed getStandardDeviationAngularRateXAsAngularSpeed() {
        return new AngularSpeed(getStandardDeviationAngularRateX(),
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated standard deviation of x coordinate of gyroscope sensed angular
     * rate.
     *
     * @param result instance where estimated standard deviation of x coordinate of
     *               sensed angular rate will be stored.
     */
    public void getStandardDeviationAngularRateXAsAngularSpeed(
            final AngularSpeed result) {
        result.setValue(getStandardDeviationAngularRateX());
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated standard deviation of y coordinate of gyroscope sensed angular
     * rate expressed in (rad/s).
     *
     * @return estimated standard deviation of y coordinate of sensed angular rate.
     */
    public double getStandardDeviationAngularRateY() {
        return Math.sqrt(mVarianceAngularRateY);
    }

    /**
     * Gets estimated standard deviation of y coordinate of gyroscope sensed angular
     * rate.
     *
     * @return estimated standard deviation of y coordinate of sensed angular rate.
     */
    public AngularSpeed getStandardDeviationAngularRateYAsAngularSpeed() {
        return new AngularSpeed(getStandardDeviationAngularRateY(),
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated standard deviation of y coordinate of gyroscope sensed angular
     * rate.
     *
     * @param result instance where estimated standard deviation of y coordinate of
     *               sensed angular rate will be stored.
     */
    public void getStandardDeviationAngularRateYAsAngularSpeed(
            final AngularSpeed result) {
        result.setValue(getStandardDeviationAngularRateY());
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated standard deviation of z coordinate of gyroscope sensed angular
     * rate expressed in (rad/s).
     *
     * @return estimated standard deviation of z coordinate of sensed angular rate.
     */
    public double getStandardDeviationAngularRateZ() {
        return Math.sqrt(mVarianceAngularRateZ);
    }

    /**
     * Gets estimated standard deviation of z coordinate of gyroscope sensed angular
     * rate.
     *
     * @return estimated standard deviation of z coordinate of sensed angular rate.
     */
    public AngularSpeed getStandardDeviationAngularRateZAsAngularSpeed() {
        return new AngularSpeed(getStandardDeviationAngularRateZ(),
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated standard deviation of z coordinate of gyroscope sensed angular
     * rate.
     *
     * @param result instance where estimated standard deviation of z coordinate of
     *               sensed angular rate will be stored.
     */
    public void getStandardDeviationAngularRateZAsAngularSpeed(
            final AngularSpeed result) {
        result.setValue(getStandardDeviationAngularRateZ());
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets average of estimated standard deviation of gyroscope sensed angular rate
     * for all coordinates expressed in radians per second (rad/s).
     *
     * @return average of estimated standard deviation of gyroscope.
     */
    public double getAverageGyroscopeStandardDeviation() {
        return (getStandardDeviationAngularRateX() + getStandardDeviationAngularRateY()
                + getStandardDeviationAngularRateZ()) / 3.0;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope sensed angular rate
     * for all coordinates.
     *
     * @return average of estimated standard deviation of gyroscope.
     */
    public AngularSpeed getAverageGyroscopeStandardDeviationAsAngularSpeed() {
        return new AngularSpeed(getAverageGyroscopeStandardDeviation(),
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets average of estimated standard deviation of gyroscope sensed angular rate
     * for all coordinates.
     *
     * @param result instance where result data will be copied to.
     */
    public void getAverageGyroscopeStandardDeviationAsAngularSpeed(
            final AngularSpeed result) {
        result.setValue(getAverageGyroscopeStandardDeviation());
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated standard deviations of accelerometer and gyroscope components
     * as a body kinematics instance.
     *
     * @return a body kinematics instance containing standard deviation values.
     */
    public BodyKinematics getStandardDeviationsAsBodyKinematics() {
        return new BodyKinematics(getStandardDeviationFx(),
                getStandardDeviationFy(),
                getStandardDeviationFz(),
                getStandardDeviationAngularRateX(),
                getStandardDeviationAngularRateY(),
                getStandardDeviationAngularRateZ());
    }

    /**
     * Gets estimated standard deviations of accelerometer and gyroscope components
     * as a body kinematics instance.
     *
     * @param result instance where data will be stored.
     */
    public void getStandardDeviationsAsBodyKinematics(final BodyKinematics result) {
        result.setSpecificForceCoordinates(getStandardDeviationFx(),
                getStandardDeviationFy(), getStandardDeviationFz());
        result.setAngularRateCoordinates(getStandardDeviationAngularRateX(),
                getStandardDeviationAngularRateY(),
                getStandardDeviationAngularRateZ());
    }

    /**
     * Gets accelerometer noise PSD (Power Spectral Density) on x axis expressed
     * in (m^2 * s^-3).
     *
     * @return accelerometer noise PSD on x axis.
     */
    public double getPSDFx() {
        return mVarianceFx * mTimeInterval;
    }

    /**
     * Gets accelerometer noise PSD (Power Spectral Density) on y axis expressed
     * in (m^2 * s^-3).
     *
     * @return accelerometer noise PSD on y axis.
     */
    public double getPSDFy() {
        return mVarianceFy * mTimeInterval;
    }

    /**
     * Gets accelerometer noise PSD (Power Spectral Density) on z axis expressed
     * in (m^2 * s^-3).
     *
     * @return accelerometer noise PSD on z axis.
     */
    public double getPSDFz() {
        return mVarianceFz * mTimeInterval;
    }

    /**
     * Gets gyroscope noise PSD (Power Spectral Density) on x axis expressed
     * in (rad^2/s).
     *
     * @return gyroscope noise PSD on x axis.
     */
    public double getPSDAngularRateX() {
        return mVarianceAngularRateX * mTimeInterval;
    }

    /**
     * Gets gyroscope noise PSD (Power Spectral Density) on y axis expressed
     * in (rad^2/s).
     *
     * @return gyroscope noise PSD on y axis.
     */
    public double getPSDAngularRateY() {
        return mVarianceAngularRateY * mTimeInterval;
    }

    /**
     * Gets gyroscope noise PSD (Power Spectral Density) on z axis expressed
     * in (rad^2/s).
     *
     * @return gyroscope noise PSD on z axis.
     */
    public double getPSDAngularRateZ() {
        return mVarianceAngularRateZ * mTimeInterval;
    }

    /**
     * Gets accelerometer noise root PSD (Power Spectral Density) on x axis
     * expressed in (m * s^-1.5).
     *
     * @return accelerometer noise root PSD on x axis.
     */
    public double getRootPSDFx() {
        return Math.sqrt(getPSDFx());
    }

    /**
     * Gets accelerometer noise root PSD (Power Spectral Density) on y axis
     * expressed in (m * s^-1.5).
     *
     * @return accelerometer noise root PSD on y axis.
     */
    public double getRootPSDFy() {
        return Math.sqrt(getPSDFy());
    }

    /**
     * Gets accelerometer noise root PSD (Power Spectral Density) on z axis
     * expressed in (m * s^-1.5).
     *
     * @return accelerometer noise root PSD on z axis.
     */
    public double getRootPSDFz() {
        return Math.sqrt(getPSDFz());
    }

    /**
     * Gets gyroscope noise root PSD (Power Spectral Density) on x axis
     * expressed in (rad * s^-0.5).
     *
     * @return gyroscope noise root PSD on x axis.
     */
    public double getRootPSDAngularRateX() {
        return Math.sqrt(getPSDAngularRateX());
    }

    /**
     * Gets gyroscope noise root PSD (Power Spectral Density) on y axis
     * expressed in (rad * s^-0.5).
     *
     * @return gyroscope noise root PSD on y axis.
     */
    public double getRootPSDAngularRateY() {
        return Math.sqrt(getPSDAngularRateY());
    }

    /**
     * Gets gyroscope noise root PSD (Power Spectral Density) on z axis
     * expressed in (rad * s^-0.5).
     *
     * @return gyroscope noise root PSD on z axis.
     */
    public double getRootPSDAngularRateZ() {
        return Math.sqrt(getPSDAngularRateZ());
    }

    /**
     * Gets average accelerometer noise PSD (Power Spectral Density) among
     * x,y,z components expressed as (m^2/s^3).
     *
     * @return average accelerometer noise PSD.
     */
    public double getAccelerometerNoisePSD() {
        return (getPSDFx() + getPSDFy() + getPSDFz()) / 3.0;
    }

    /**
     * Gets average accelerometer noise root PSD (Power Spectral Density) among
     * x,y,z components expressed as (m * s^-1.5).
     *
     * @return average accelerometer noise root PSD.
     */
    public double getAccelerometerNoiseRootPSD() {
        return Math.sqrt(getAccelerometerNoisePSD());
    }

    /**
     * Gets average gyroscope noise PSD (Power Spectral Density) among
     * x,y,z components expressed in (rad^2/s).
     *
     * @return average gyroscope noise PSD.
     */
    public double getGyroNoisePSD() {
        return (getPSDAngularRateX() + getPSDAngularRateY() + getPSDAngularRateZ())
                / 3.0;
    }

    /**
     * Gets average gyroscope noise root PSD (Power Spectral Density) among
     * x,y,z components expressed in (rad * s^-0.5).
     *
     * @return average gyroscope noise root PSD.
     */
    public double getGyroNoiseRootPSD() {
        return Math.sqrt(getGyroNoisePSD());
    }

    /**
     * Gets estimated bias of accelerometer sensed specific force
     * expressed in meters per squared second (m/s^2) as a 3x1 matrix column vector.
     *
     * @return estimated bias of accelerometer sensed specific force.
     */
    public Matrix getAccelerometerBias() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 5944
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1327
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3317
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3275
        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    public double[] getBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mBiasX;
        result[1] = mBiasY;
        result[2] = mBiasZ;
    }

    /**
     * Sets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known bias to be set
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setBias(final double[] bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mBiasX = bias[0];
        mBiasY = bias[1];
        mBiasZ = bias[2];
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @return known bias as a column matrix.
     */
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mBiasX);
        result.setElementAtIndex(1, mBiasY);
        result.setElementAtIndex(2, mBiasZ);
    }

    /**
     * Sets known bias as an array.
     *
     * @param bias bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setBias(final Matrix bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS
                || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mBiasX = bias.getElementAtIndex(0);
        mBiasY = bias.getElementAtIndex(1);
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6384
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 689
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1361
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2120
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2122
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 709
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3328
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3373
        mInitialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of initial bias.
     */
    public double[] getInitialBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mInitialBiasX;
        result[1] = mInitialBiasY;
        result[2] = mInitialBiasZ;
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mInitialBiasX = initialBias[0];
        mInitialBiasY = initialBias[1];
        mInitialBiasZ = initialBias[2];
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     *
     * @return initial bias to be used to find a solution as a column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialBiasX);
        result.setElementAtIndex(1, mInitialBiasY);
        result.setElementAtIndex(2, mInitialBiasZ);
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS
                || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mInitialBiasX = initialBias.getElementAtIndex(0);
        mInitialBiasY = initialBias.getElementAtIndex(1);
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5081
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5391
        setResult(m);
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter when G-dependent cross biases
     * are taken into account.
     *
     * @throws AlgebraException                              if provided accelerometer cross coupling
     *                                                       errors are not valid.
     * @throws InvalidSourceAndDestinationFrameTypeException never happens
     */
    private void setInputDataWithGDependentCrossBiases()
            throws AlgebraException,
            InvalidSourceAndDestinationFrameTypeException {
        // compute reference frame at current position
        final NEDPosition nedPosition = getNedPosition();
        final CoordinateTransformation nedC = new CoordinateTransformation(
                FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
        final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
                .convertNEDtoECEFAndReturnNew(nedFrame);
        final BodyKinematics refKinematics = ECEFKinematicsEstimator
                .estimateKinematicsAndReturnNew(mTimeInterval, ecefFrame,
                        ecefFrame);

        final double refAngularRateX = refKinematics.getAngularRateX();
        final double refAngularRateY = refKinematics.getAngularRateY();
        final double refAngularRateZ = refKinematics.getAngularRateZ();

        final double w2 = mTurntableRotationRate * mTurntableRotationRate;

        final int numMeasurements = mMeasurements.size();
        final Matrix x = new Matrix(numMeasurements, 2 * BodyKinematics.COMPONENTS);
        final double[] y = new double[numMeasurements];
        final double[] angularRateStandardDeviations = new double[numMeasurements];
        int i = 0;
        for (final StandardDeviationBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();

            final double angularRateX = measuredKinematics.getAngularRateX();
            final double angularRateY = measuredKinematics.getAngularRateY();
            final double angularRateZ = measuredKinematics.getAngularRateZ();

            final double fX = measuredKinematics.getFx();
            final double fY = measuredKinematics.getFy();
            final double fZ = measuredKinematics.getFz();

            x.setElementAt(i, 0, angularRateX - refAngularRateX);
            x.setElementAt(i, 1, angularRateY - refAngularRateY);
            x.setElementAt(i, 2, angularRateZ - refAngularRateZ);

            x.setElementAt(i, 3, fX);
            x.setElementAt(i, 4, fY);
            x.setElementAt(i, 5, fZ);

            y[i] = w2;

            angularRateStandardDeviations[i] =
                    measurement.getAngularRateStandardDeviation();

            i++;
        }

        mFitter.setInputData(x, y, angularRateStandardDeviations);

        mBa = getAccelerometerBiasAsMatrix();
        mMa = getAccelerometerMa();
        mAccelerationFixer.setBias(mBa);
        mAccelerationFixer.setCrossCouplingErrors(mMa);
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter when G-dependent cross biases
     * are ignored.
     *
     * @throws AlgebraException                              if provided accelerometer cross coupling
     *                                                       errors are not valid.
     * @throws InvalidSourceAndDestinationFrameTypeException never happens.
     */
    private void setInputData() throws AlgebraException,
            InvalidSourceAndDestinationFrameTypeException {

        // compute reference frame at current position
        final NEDPosition nedPosition = getNedPosition();
        final CoordinateTransformation nedC = new CoordinateTransformation(
                FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
        final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
                .convertNEDtoECEFAndReturnNew(nedFrame);
        final BodyKinematics refKinematics = ECEFKinematicsEstimator
                .estimateKinematicsAndReturnNew(mTimeInterval, ecefFrame,
                        ecefFrame);

        final double refAngularRateX = refKinematics.getAngularRateX();
        final double refAngularRateY = refKinematics.getAngularRateY();
        final double refAngularRateZ = refKinematics.getAngularRateZ();

        final double w2 = mTurntableRotationRate * mTurntableRotationRate;

        final int numMeasurements = mMeasurements.size();
        final Matrix x = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
        final double[] y = new double[numMeasurements];
        final double[] angularRateStandardDeviations = new double[numMeasurements];
        int i = 0;
        for (final StandardDeviationBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();

            final double angularRateX = measuredKinematics.getAngularRateX();
            final double angularRateY = measuredKinematics.getAngularRateY();
            final double angularRateZ = measuredKinematics.getAngularRateZ();

            x.setElementAt(i, 0,
                    angularRateX - refAngularRateX);
            x.setElementAt(i, 1,
                    angularRateY - refAngularRateY);
            x.setElementAt(i, 2,
                    angularRateZ - refAngularRateZ);

            y[i] = w2;

            angularRateStandardDeviations[i] =
                    measurement.getAngularRateStandardDeviation();

            i++;
        }

        mFitter.setInputData(x, y, angularRateStandardDeviations);

        mBa = getAccelerometerBiasAsMatrix();
        mMa = getAccelerometerMa();
        mAccelerationFixer.setBias(mBa);
        mAccelerationFixer.setCrossCouplingErrors(mMa);
    }

    /**
     * Converts provided NED position expressed in terms of latitude, longitude and height respect
     * mean Earth surface, to position expressed in ECEF coordinates.
     *
     * @param position NED position to be converted.
     * @return converted position expressed in ECEF coordinates.
     */
    private static ECEFPosition convertPosition(final NEDPosition position) {
        final ECEFVelocity velocity = new ECEFVelocity();
        final ECEFPosition result = new ECEFPosition();
        NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                position.getLatitude(), position.getLongitude(), position.getHeight(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(),
                acceleration.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second.
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value.
     */
    private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
        return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(),
                angularSpeed.getUnit(), AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Converts time instance to seconds.
     *
     * @param time time instance to be converted.
     * @return converted value.
     */
    private static double convertTime(final Time time) {
        return TimeConverter.convert(time.getValue().doubleValue(),
                time.getUnit(), TimeUnit.SECOND);
    }

    /**
     * Makes proper conversion of internal cross-coupling and g-dependent
     * cross bias matrices.
     *
     * @param m internal scaling and cross-coupling matrix.
     * @param g internal g-dependent cross bias matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m, final Matrix g)
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1222
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1250
                                   final Acceleration biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        internalSetBiasCoordinates(biasX, biasY, biasZ);
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    public double[] getBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mBiasX;
        result[1] = mBiasY;
        result[2] = mBiasZ;
    }

    /**
     * Sets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setBias(final double[] bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        internalSetBias(bias);
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @return known accelerometer bias as a column matrix.
     */
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mBiasX);
        result.setElementAtIndex(1, mBiasY);
        result.setElementAtIndex(2, mBiasZ);
    }

    /**
     * Sets known accelerometer bias as a column matrix.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setBias(final Matrix bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        internalSetBias(bias);
    }

    /**
     * Gets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 5944
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1327
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2083
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2089
        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    public double[] getBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mBiasX;
        result[1] = mBiasY;
        result[2] = mBiasZ;
    }

    /**
     * Sets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known bias to be set
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setBias(final double[] bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mBiasX = bias[0];
        mBiasY = bias[1];
        mBiasZ = bias[2];
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @return known bias as a column matrix.
     */
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mBiasX);
        result.setElementAtIndex(1, mBiasY);
        result.setElementAtIndex(2, mBiasZ);
    }

    /**
     * Sets known bias as an array.
     *
     * @param bias bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setBias(final Matrix bias) throws LockedException {
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 1678
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 1852
    public boolean getFrame(ECEFFrame result) {
        if (mBodyToEcefCoordinateTransformationMatrix != null) {
            try {
                result.setCoordinateTransformation(getC());
            } catch (final InvalidSourceAndDestinationFrameTypeException
                    | InvalidRotationMatrixException e) {
                return false;
            }
            result.setCoordinates(mX, mY, mZ);
            result.setVelocityCoordinates(mVx, mVy, mVz);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets body to ECEF frame containing coordinate transformation, position and
     * velocity.
     *
     * @return body to ECEF frame.
     */
    public ECEFFrame getFrame() {
        if (mBodyToEcefCoordinateTransformationMatrix != null) {
            try {
                return new ECEFFrame(mX, mY, mZ, mVx, mVy, mVz, getC());
            } catch (final InvalidSourceAndDestinationFrameTypeException
                    | InvalidRotationMatrixException e) {
                return null;
            }
        } else {
            return null;
        }
    }

    /**
     * Sets body to ECEF frame containing coordinate transformation, position and
     * velocity.
     *
     * @param frame body to ECEF frame to be set.
     */
    public void setFrame(final ECEFFrame frame) {
        mX = frame.getX();
        mY = frame.getY();
        mZ = frame.getZ();

        mVx = frame.getVx();
        mVy = frame.getVy();
        mVz = frame.getVz();

        if (mBodyToEcefCoordinateTransformationMatrix != null) {
            frame.getCoordinateTransformationMatrix(mBodyToEcefCoordinateTransformationMatrix);
        } else {
            mBodyToEcefCoordinateTransformationMatrix = frame.getCoordinateTransformationMatrix();
        }
    }

    /**
     * Gets estimated accelerometer bias resolved around x axis.
     *
     * @param result instance where estimated accelerometer bias resolved around
     *               x axis will be stored.
     */
    public void getAccelerationBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerationBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated accelerometer bias resolved around x axis.
     *
     * @return estimated accelerometer bias resolved around x axis.
     */
    public Acceleration getAccelerationBiasXAsAcceleration() {
        return new Acceleration(mAccelerationBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets estimated accelerometer bias resolved around x axis.
     *
     * @param accelerationBiasX estimated accelerometer bias resolved
     *                          around x axis.
     */
    public void setAccelerationBiasX(final Acceleration accelerationBiasX) {
        mAccelerationBiasX = AccelerationConverter.convert(
                accelerationBiasX.getValue().doubleValue(),
                accelerationBiasX.getUnit(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated accelerometer bias resolved around y axis.
     *
     * @param result instance where estimated accelerometer bias resolved around
     *               y axis will be stored.
     */
    public void getAccelerationBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerationBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated accelerometer bias resolved around y axis.
     *
     * @return estimated accelerometer bias resolved around y axis.
     */
    public Acceleration getAccelerationBiasYAsAcceleration() {
        return new Acceleration(mAccelerationBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets estimated accelerometer bias resolved around y axis.
     *
     * @param accelerationBiasY estimated accelerometer bias resolved
     *                          around y axis.
     */
    public void setAccelerationBiasY(final Acceleration accelerationBiasY) {
        mAccelerationBiasY = AccelerationConverter.convert(
                accelerationBiasY.getValue().doubleValue(),
                accelerationBiasY.getUnit(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated accelerometer bias resolved around z axis.
     *
     * @param result instance where estimated accelerometer bias resolved around
     *               z axis will be stored.
     */
    public void getAccelerationBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mAccelerationBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated accelerometer bias resolved around z axis.
     *
     * @return estimated accelerometer bias resolved around z axis.
     */
    public Acceleration getAccelerationBiasZAsAcceleration() {
        return new Acceleration(mAccelerationBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets estimated accelerometer bias resolved around z axis.
     *
     * @param accelerationBiasZ estimated accelerometer bias resolved
     *                          around z axis.
     */
    public void setAccelerationBiasZ(final Acceleration accelerationBiasZ) {
        mAccelerationBiasZ = AccelerationConverter.convert(
                accelerationBiasZ.getValue().doubleValue(),
                accelerationBiasZ.getUnit(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets estimated accelerometer bias coordinates.
     *
     * @param accelerationBiasX estimated accelerometer bias resolved around x axis.
     * @param accelerationBiasY estimated accelerometer bias resolved around y axis.
     * @param accelerationBiasZ estimated accelerometer bias resolved around z axis.
     */
    public void setAccelerationBiasCoordinates(
            final Acceleration accelerationBiasX, final Acceleration accelerationBiasY,
            final Acceleration accelerationBiasZ) {
        setAccelerationBiasX(accelerationBiasX);
        setAccelerationBiasY(accelerationBiasY);
        setAccelerationBiasZ(accelerationBiasZ);
    }

    /**
     * Gets estimated gyroscope bias resolved around x axis.
     *
     * @param result instance where estimated gyroscope bias resolved around x axis will
     *               be stored.
     */
    public void getAngularSpeedGyroBiasX(final AngularSpeed result) {
        result.setValue(mGyroBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated gyroscope bias resolved around x axis.
     *
     * @return estimated gyroscope bias resolved around x axis.
     */
    public AngularSpeed getAngularSpeedGyroBiasX() {
        return new AngularSpeed(mGyroBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets estimated gyroscope bias resolved around x axis.
     *
     * @param gyroBiasX estimated gyroscope bias resolved around x axis.
     */
    public void setGyroBiasX(final AngularSpeed gyroBiasX) {
        mGyroBiasX = AngularSpeedConverter.convert(
                gyroBiasX.getValue().doubleValue(),
                gyroBiasX.getUnit(), AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated gyroscope bias resolved around y axis.
     *
     * @param result instance where estimated gyroscope bias resolved around y axis will
     *               be stored.
     */
    public void getAngularSpeedGyroBiasY(final AngularSpeed result) {
        result.setValue(mGyroBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated gyroscope bias resolved around y axis.
     *
     * @return estimated gyroscope bias resolved around y axis.
     */
    public AngularSpeed getAngularSpeedGyroBiasY() {
        return new AngularSpeed(mGyroBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets estimated gyroscope bias resolved around y axis.
     *
     * @param gyroBiasY estimated gyroscope bias resolved around y axis.
     */
    public void setGyroBiasY(final AngularSpeed gyroBiasY) {
        mGyroBiasY = AngularSpeedConverter.convert(
                gyroBiasY.getValue().doubleValue(),
                gyroBiasY.getUnit(), AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated gyroscope bias resolved around z axis.
     *
     * @param result instance where estimated gyroscope bias resolved around z axis will
     *               be stored.
     */
    public void getAngularSpeedGyroBiasZ(final AngularSpeed result) {
        result.setValue(mGyroBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated gyroscope bias resolved around z axis.
     *
     * @return estimated gyroscope bias resolved around z axis.
     */
    public AngularSpeed getAngularSpeedGyroBiasZ() {
        return new AngularSpeed(mGyroBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets estimated gyroscope bias resolved around z axis.
     *
     * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
     */
    public void setGyroBiasZ(final AngularSpeed gyroBiasZ) {
        mGyroBiasZ = AngularSpeedConverter.convert(
                gyroBiasZ.getValue().doubleValue(),
                gyroBiasZ.getUnit(), AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets estimated gyroscope bias coordinates.
     *
     * @param gyroBiasX estimated gyroscope bias resolved around x axis.
     * @param gyroBiasY estimated gyroscope bias resolved around y axis.
     * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
     */
    public void setGyroBiasCoordinates(
            final AngularSpeed gyroBiasX, final AngularSpeed gyroBiasY,
            final AngularSpeed gyroBiasZ) {
        setGyroBiasX(gyroBiasX);
        setGyroBiasY(gyroBiasY);
        setGyroBiasZ(gyroBiasZ);
    }

    /**
     * Copies this instance data into provided instance.
     *
     * @param output destination instance where data will be copied to.
     */
    public void copyTo(final INSLooselyCoupledKalmanState output) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1265
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2157
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    public double[] getEstimatedHardIron() {
        return mEstimatedHardIron;
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where estimated magnetometer biases will be
     *               stored.
     * @return true if result instance was updated, false otherwise (when
     * estimation is not yet available).
     */
    public boolean getEstimatedHardIron(final double[] result) {
        if (mEstimatedHardIron != null) {
            System.arraycopy(mEstimatedHardIron, 0, result,
                    0, mEstimatedHardIron.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @return column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases.
     */
    public Matrix getEstimatedHardIronAsMatrix() {
        return mEstimatedHardIron != null ? Matrix.newFromArray(mEstimatedHardIron) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedHardIronAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedHardIron != null) {
            result.fromArray(mEstimatedHardIron);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return x coordinate of estimated magnetometer bias or null if not
     * available.
     */
    public Double getEstimatedHardIronX() {
        return mEstimatedHardIron != null ? mEstimatedHardIron[0] : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return y coordinate of estimated magnetometer bias or null if not
     * available.
     */
    public Double getEstimatedHardIronY() {
        return mEstimatedHardIron != null ? mEstimatedHardIron[1] : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return z coordinate of estimated magnetometer bias or null if not
     * available.
     */
    public Double getEstimatedHardIronZ() {
        return mEstimatedHardIron != null ? mEstimatedHardIron[2] : null;
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1299
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 560
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1370
    public void setHardIron(
            final double hardIronX, final double hardIronY,
            final double hardIronZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mHardIronX = hardIronX;
        mHardIronY = hardIronY;
        mHardIronZ = hardIronZ;
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx,
            final double initialSy,
            final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy,
            final double initialMxz,
            final double initialMyx,
            final double initialMyz,
            final double initialMzx,
            final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx,
            final double initialSy,
            final double initialSz,
            final double initialMxy,
            final double initialMxz,
            final double initialMyx,
            final double initialMyz,
            final double initialMzx,
            final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of known bias.
     */
    public double[] getHardIron() {
        final double[] result = new double[
                BodyMagneticFluxDensity.COMPONENTS];
        getHardIron(result);
        return result;
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mHardIronX;
        result[1] = mHardIronY;
        result[2] = mHardIronZ;
    }

    /**
     * Sets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param hardIron known hard-iron.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setHardIron(final double[] hardIron)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mHardIronX = hardIron[0];
        mHardIronY = hardIron[1];
        mHardIronZ = hardIron[2];
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @return known hard-iron bias as a column matrix.
     */
    public Matrix getHardIronAsMatrix() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 443
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3252
        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z component of estimated gyroscope
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasX() {
        return mEstimatedBiases != null ? mEstimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasY() {
        return mEstimatedBiases != null ? mEstimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasZ() {
        return mEstimatedBiases != null ? mEstimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[0],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[1],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[2],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1706
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 1722
        mHardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMm(final Matrix initialMm)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS ||
                initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMm.getElementAtIndex(0);
        mInitialMyx = initialMm.getElementAtIndex(1);
        mInitialMzx = initialMm.getElementAtIndex(2);

        mInitialMxy = initialMm.getElementAtIndex(3);
        mInitialSy = initialMm.getElementAtIndex(4);
        mInitialMzy = initialMm.getElementAtIndex(5);

        mInitialMxz = initialMm.getElementAtIndex(6);
        mInitialMyz = initialMm.getElementAtIndex(7);
        mInitialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken.
     *
     * @return position where body magnetic flux density measurements have
     * been taken.
     */
    public NEDPosition getNedPosition() {
        return mPosition;
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken.
     *
     * @param position position where body magnetic flux density measurements
     *                 have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @return position where body magnetic flux density measurements have
     * been taken or null if not available.
     */
    public ECEFPosition getEcefPosition() {
        if (mPosition != null) {
            final ECEFPosition result = new ECEFPosition();
            getEcefPosition(result);
            return result;
        } else {
            return null;
        }
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if ECEF position could be computed, false otherwise.
     */
    public boolean getEcefPosition(final ECEFPosition result) {

        if (mPosition != null) {
            final ECEFVelocity velocity = new ECEFVelocity();
            NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                    mPosition.getLatitude(), mPosition.getLongitude(),
                    mPosition.getHeight(), 0.0, 0.0, 0.0,
                    result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param position position where body magnetic flux density have been
     *                 taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Gets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @return timestamp expressed as decimal year or null if not defined.
     */
    public Double getYear() {
        return mYear;
    }

    /**
     * Sets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @param year timestamp expressed as decimal year.
     * @throws LockedException if calibrator is currently running.
     */
    public void setYear(final Double year) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = year;
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param timestampMillis a timestamp expressed in milliseocnds since
     *                        epoch time (January 1st, 1970 at midnight).
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Long timestampMillis) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(timestampMillis);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param date a date instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Date date) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(date);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param calendar a calendar instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final GregorianCalendar calendar)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(calendar);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @param measurements collection of body magnetic flux density
     *                     measurements at a known position and timestamp
     *                     with unknown orientations.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMeasurements(
            final Collection<StandardDeviationBodyMagneticFluxDensity> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer.
     * When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer, false otherwise.
     */
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mm matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for
     *                       accelerometer, gyroscope and magnetometer, false
     *                       otherwise.
     * @throws LockedException if estimator is currently running.
     */
    public void setCommonAxisUsed(final boolean commonAxisUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    public KnownHardIronPositionAndInstantMagnetometerCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1777
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1793
        mHardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMm(final Matrix initialMm)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS ||
                initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMm.getElementAtIndex(0);
        mInitialMyx = initialMm.getElementAtIndex(1);
        mInitialMzx = initialMm.getElementAtIndex(2);

        mInitialMxy = initialMm.getElementAtIndex(3);
        mInitialSy = initialMm.getElementAtIndex(4);
        mInitialMzy = initialMm.getElementAtIndex(5);

        mInitialMxz = initialMm.getElementAtIndex(6);
        mInitialMyz = initialMm.getElementAtIndex(7);
        mInitialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken.
     *
     * @return position where body magnetic flux density measurements have
     * been taken.
     */
    public NEDPosition getNedPosition() {
        return mPosition;
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken.
     *
     * @param position position where body magnetic flux density measurements
     *                 have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @return position where body magnetic flux density measurements have
     * been taken or null if not available.
     */
    public ECEFPosition getEcefPosition() {
        if (mPosition != null) {
            final ECEFPosition result = new ECEFPosition();
            getEcefPosition(result);
            return result;
        } else {
            return null;
        }
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if ECEF position could be computed, false otherwise.
     */
    public boolean getEcefPosition(final ECEFPosition result) {

        if (mPosition != null) {
            final ECEFVelocity velocity = new ECEFVelocity();
            NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                    mPosition.getLatitude(), mPosition.getLongitude(),
                    mPosition.getHeight(), 0.0, 0.0, 0.0,
                    result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param position position where body magnetic flux density have been
     *                 taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Gets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @return timestamp expressed as decimal year or null if not defined.
     */
    public Double getYear() {
        return mYear;
    }

    /**
     * Sets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @param year timestamp expressed as decimal year.
     * @throws LockedException if calibrator is currently running.
     */
    public void setYear(final Double year) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = year;
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param timestampMillis a timestamp expressed in milliseocnds since
     *                        epoch time (January 1st, 1970 at midnight).
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Long timestampMillis) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(timestampMillis);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param date a date instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Date date) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(date);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param calendar a calendar instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final GregorianCalendar calendar)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(calendar);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    public List<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @param measurements collection of body magnetic flux density
     *                     measurements at a known position and timestamp
     *                     with unknown orientations.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMeasurements(
            final List<StandardDeviationBodyMagneticFluxDensity> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer.
     * When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer, false otherwise.
     */
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mm matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for
     *                       accelerometer, gyroscope and magnetometer, false
     *                       otherwise.
     * @throws LockedException if estimator is currently running.
     */
    public void setCommonAxisUsed(final boolean commonAxisUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    public RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3701
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3719
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @return constant rotation rate of turntable.
     */
    public double getTurntableRotationRate() {
        return mTurntableRotationRate;
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final double turntableRotationRate) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (turntableRotationRate <= 0.0) {
            throw new IllegalArgumentException();
        }

        mTurntableRotationRate = turntableRotationRate;
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @return constant rotation rate of turntable.
     */
    public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
        return new AngularSpeed(mTurntableRotationRate,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @param result instance where result will be stored.
     */
    public void getTurntableRotationRateAsAngularSpeed(
            final AngularSpeed result) {
        result.setValue(mTurntableRotationRate);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final AngularSpeed turntableRotationRate)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setTurntableRotationRate(
                convertAngularSpeed(turntableRotationRate));
    }

    /**
     * Gets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @return time interval between measurements.
     */
    public double getTimeInterval() {
        return mTimeInterval;
    }

    /**
     * Sets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTimeInterval(final double timeInterval)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (timeInterval <= 0.0) {
            throw new IllegalArgumentException();
        }
        mTimeInterval = timeInterval;
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @return time interval between measurements.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(mTimeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @param result instance where result will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(mTimeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between measurements being captured.
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTimeInterval(final Time timeInterval)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setTimeInterval(convertTime(timeInterval));
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3659
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3764
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @return constant rotation rate of turntable.
     */
    public double getTurntableRotationRate() {
        return mTurntableRotationRate;
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final double turntableRotationRate) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (turntableRotationRate <= 0.0) {
            throw new IllegalArgumentException();
        }

        mTurntableRotationRate = turntableRotationRate;
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @return constant rotation rate of turntable.
     */
    public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
        return new AngularSpeed(mTurntableRotationRate,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @param result instance where result will be stored.
     */
    public void getTurntableRotationRateAsAngularSpeed(
            final AngularSpeed result) {
        result.setValue(mTurntableRotationRate);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final AngularSpeed turntableRotationRate)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setTurntableRotationRate(
                convertAngularSpeed(turntableRotationRate));
    }

    /**
     * Gets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @return time interval between measurements.
     */
    public double getTimeInterval() {
        return mTimeInterval;
    }

    /**
     * Sets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTimeInterval(final double timeInterval)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (timeInterval <= 0.0) {
            throw new IllegalArgumentException();
        }
        mTimeInterval = timeInterval;
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @return time interval between measurements.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(mTimeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @param result instance where result will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(mTimeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between measurements being captured.
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTimeInterval(final Time timeInterval)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setTimeInterval(convertTime(timeInterval));
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    public List<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 428
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2992
        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
        return mEstimatedBiases != null ? mEstimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFy() {
        return mEstimatedBiases != null ? mEstimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFz() {
        return mEstimatedBiases != null ? mEstimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFxAsAcceleration() {
        return mEstimatedBiases != null ?
                new Acceleration(mEstimatedBiases[0],
                        AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[0]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFyAsAcceleration() {
        return mEstimatedBiases != null ?
                new Acceleration(mEstimatedBiases[1],
                        AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[1]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFzAsAcceleration() {
        return mEstimatedBiases != null ?
                new Acceleration(mEstimatedBiases[2],
                        AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[2]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2837
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4297
                | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasX() {
        return mEstimatedBiases != null ? mEstimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasY() {
        return mEstimatedBiases != null ? mEstimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasZ() {
        return mEstimatedBiases != null ? mEstimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[0],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[1],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[2],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
        return mEstimatedChiSq;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxisAndGDependentCrossBiases()
            throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator.java 1155
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator.java 1137
            final SequentialRobustMixedRadioSourceEstimatorListener<S, P> listener) {
        this(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether estimator is locked during estimation.
     *
     * @return true if estimator is locked, false otherwise.
     */
    public boolean isLocked() {
        return mLocked;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * estimation.
     *
     * @return amount of progress variation before notifying a progress change during
     * estimation.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * estimation.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during estimation.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if this estimator is locked.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Gets robust method used for robust position estimation using ranging data.
     *
     * @return robust method used for robust position estimation.
     */
    public RobustEstimatorMethod getRangingRobustMethod() {
        return mRangingRobustMethod;
    }

    /**
     * Sets robust method used for robust position estimation using ranging data.
     *
     * @param rangingRobustMethod robust method used for robust position estimation.
     * @throws LockedException if estimator is locked.
     */
    public void setRangingRobustMethod(
            final RobustEstimatorMethod rangingRobustMethod) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mRangingRobustMethod = rangingRobustMethod;
    }

    /**
     * Gets robust method used for pathloss exponent and transmitted power estimation
     * using RSSI data.
     *
     * @return robust method used for pathloss exponent and transmitted power
     * estimation.
     */
    public RobustEstimatorMethod getRssiRobustMethod() {
        return mRssiRobustMethod;
    }

    /**
     * Sets robust method used for pathloss exponent and transmitted power estimation
     * using RSSI data.
     *
     * @param rssiRobustMethod robust method used for pathloss exponent and transmitted
     *                         power estimation.
     * @throws LockedException if estimator is locked.
     */
    public void setRssiRobustMethod(
            final RobustEstimatorMethod rssiRobustMethod) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mRssiRobustMethod = rssiRobustMethod;
    }

    /**
     * Gets size of subsets to be checked during ranging robust estimation.
     *
     * @return size of subsets to be checked during ranging robust estimation.
     */
    public int getRangingPreliminarySubsetSize() {
        return mRangingPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during ranging robust estimation.
     *
     * @param rangingPreliminarySubsetSize size of subsets to be checked during
     *                                     ranging robust estimation.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is less than {@link #getMinReadings()}.
     */
    public void setRangingPreliminarySubsetSize(
            final int rangingPreliminarySubsetSize) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rangingPreliminarySubsetSize < getMinReadings()) {
            throw new IllegalArgumentException();
        }

        mRangingPreliminarySubsetSize = rangingPreliminarySubsetSize;
    }

    /**
     * Gets size of subsets to be checked during RSSI robust estimation.
     *
     * @return size of subsets to be checked during RSSI robust estimation.
     */
    public int getRssiPreliminarySubsetSize() {
        return mRssiPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during RSSI robust estimation.
     *
     * @param rssiPreliminarySubsetSize size of subsets to be checked during
     *                                  RSSI robust estimation.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is less than {@link #getMinReadings()}.
     */
    public void setRssiPreliminarySubsetSize(
            final int rssiPreliminarySubsetSize) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rssiPreliminarySubsetSize < getMinReadings()) {
            throw new IllegalArgumentException();
        }

        mRssiPreliminarySubsetSize = rssiPreliminarySubsetSize;
    }

    /**
     * Gets threshold to determine when samples are inliers or not, used during robust
     * position estimation.
     * If not defined, default threshold will be used.
     *
     * @return threshold for ranging estimation or null.
     */
    public Double getRangingThreshold() {
        return mRangingThreshold;
    }

    /**
     * Sets threshold to determine when samples are inliers or not, used during robust
     * position estimation.
     * If not defined, default threshold will be used.
     *
     * @param rangingThreshold threshold for ranging estimation or null.
     * @throws LockedException if estimator is locked.
     */
    public void setRangingThreshold(final Double rangingThreshold)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mRangingThreshold = rangingThreshold;
    }

    /**
     * Gets threshold to determine when samples are inliers or not, used during robust
     * pathloss exponent and transmitted power estimation.
     * If not defined, default threshold will be used.
     *
     * @return threshold for RSSI estimation or null.
     */
    public Double getRssiThreshold() {
        return mRssiThreshold;
    }

    /**
     * Sets threshold to determine when samples are inliers or not, used during robust
     * pathloss exponent and transmitted power estimation.
     * If not defined, default threshold will be used.
     *
     * @param rssiThreshold threshold for RSSI estimation or null.
     * @throws LockedException if estimator is locked.
     */
    public void setRssiThreshold(final Double rssiThreshold)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mRssiThreshold = rssiThreshold;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%) for robust position estimation. The amount of
     * confidence indicates the probability that the estimated result is correct.
     * Usually this value will be close to 1.0, but not exactly 1.0.
     *
     * @return amount of confidence for robust position estimation as a value
     * between 0.0 and 1.0.
     */
    public double getRangingConfidence() {
        return mRangingConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%) for robust position estimation. The amount of confidence
     * indicates the probability that the estimated result is correct. Usually this
     * value will be close to 1.0, but not exactly 1.0.
     *
     * @param rangingConfidence confidence to be set for robust position estimation
     *                          as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if estimator is locked.
     */
    public void setRangingConfidence(
            final double rangingConfidence) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rangingConfidence < MIN_CONFIDENCE || rangingConfidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mRangingConfidence = rangingConfidence;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%) for pathloss exponent and transmitted power
     * estimation. The amount of confidence indicates the probability that the
     * estimated result is correct.
     * Usually this value will be close to 1.0, but not exactly 1.0.
     *
     * @return amount of confidence for robust pathloss exponent and transmitted power
     * estimation as a value between 0.0 and 1.0.
     */
    public double getRssiConfidence() {
        return mRssiConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%) for pathloss exponent and transmitted power
     * estimation. The amount of confidence indicates the probability that the
     * estimated result is correct. Usually this value will be close to 10.0, but
     * not exactly 1.0.
     *
     * @param rssiConfidence confidence to be set for robust pathloss exponent and
     *                       transmitted power estimation as a value between 0.0 and
     *                       1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if estimator is locked.
     */
    public void setRssiConfidence(
            final double rssiConfidence) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rssiConfidence < MIN_CONFIDENCE || rssiConfidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mRssiConfidence = rssiConfidence;
    }

    /**
     * Returns maximum allowed number of iterations for robust position estimation. If
     * maximum allowed number of iterations is achieved without converging to a result
     * when calling estimate(), a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations for position estimation.
     */
    public int getRangingMaxIterations() {
        return mRangingMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations for robust position estimation. When
     * the maximum number of iterations is exceeded, an approximate result might be
     * available for retrieval.
     *
     * @param rangingMaxIterations maximum allowed number of iterations to be set
     *                             for position estimation.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if this estimator is locked.
     */
    public void setRangingMaxIterations(
            final int rangingMaxIterations) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rangingMaxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mRangingMaxIterations = rangingMaxIterations;
    }

    /**
     * Returns maximum allowed number of iterations for robust pathloss exponent and
     * transmitted power estimation. If maximum allowed number of iterations is achieved
     * without converging to a result when calling estimate(), a RobustEstimatorException
     * will be raised.
     *
     * @return maximum allowed number of iterations for pathloss exponent and transmitted
     * power estimation.
     */
    public int getRssiMaxIterations() {
        return mRssiMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations for robust pathloss exponent and
     * transmitted power estimation. When the maximum number of iterations is exceeded,
     * an approximate result might be available for retrieval.
     *
     * @param rssiMaxIterations maximum allowed number of iterations to be set for
     *                          pathloss exponent and transmitted power estimation.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if this estimator is locked.
     */
    public void setRssiMaxIterations(
            final int rssiMaxIterations) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rssiMaxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mRssiMaxIterations = rssiMaxIterations;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if estimator is locked.
     */
    public void setResultRefined(
            final boolean refineResult) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setCovarianceKept(
            final boolean keepCovariance) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Gets signal readings belonging to the same radio source.
     *
     * @return signal readings belonging to the same radio source.
     */
    public List<? extends ReadingLocated<P>> getReadings() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2996
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1590
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4402
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasX() {
        return mEstimatedBiases != null ? mEstimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasY() {
        return mEstimatedBiases != null ? mEstimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasZ() {
        return mEstimatedBiases != null ? mEstimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[0],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[1],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[2],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredSequences()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredSequences}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than
     *                                  {@link #getMinimumRequiredSequences}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredSequences()) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6614
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7221
            mRunning = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
        return mEstimatedChiSq;
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter.
     *
     * @throws WrongSizeException never happens.
     */
    private void setInputData() throws WrongSizeException {

        final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
                mPosition.getX(), mPosition.getY(), mPosition.getZ());
        final double g = gravity.getNorm();
        final double g2 = g * g;

        final int numMeasurements = mMeasurements.size();
        final Matrix x = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
        final double[] y = new double[numMeasurements];
        final double[] specificForceStandardDeviations = new double[numMeasurements];
        int i = 0;
        for (final StandardDeviationBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();

            final double fmeasX = measuredKinematics.getFx();
            final double fmeasY = measuredKinematics.getFy();
            final double fmeasZ = measuredKinematics.getFz();

            x.setElementAt(i, 0, fmeasX);
            x.setElementAt(i, 1, fmeasY);
            x.setElementAt(i, 2, fmeasZ);

            y[i] = g2;

            specificForceStandardDeviations[i] =
                    measurement.getSpecificForceStandardDeviation();

            i++;
        }

        mFitter.setInputData(x, y, specificForceStandardDeviations);
    }

    /**
     * Converts provided NED position expressed in terms of latitude, longitude and height respect
     * mean Earth surface, to position expressed in ECEF coordinates.
     *
     * @param position NED position to be converted.
     * @return converted position expressed in ECEF coordinates.
     */
    private static ECEFPosition convertPosition(final NEDPosition position) {
        final ECEFVelocity velocity = new ECEFVelocity();
        final ECEFPosition result = new ECEFPosition();
        NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                position.getLatitude(), position.getLongitude(), position.getHeight(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(),
                acceleration.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws FittingException                         if Levenberg-Marquardt fails for numerical reasons.
     * @throws AlgebraException                         if there are numerical instabilities that prevent
     *                                                  matrix inversion.
     * @throws com.irurueta.numerical.NotReadyException never happens.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // For convergence purposes of the Levenberg-Marquardt algorithm, the
        // accelerometer model can be better expressed as:
        // fmeas = T*K*(ftrue + b)
        // fmeas = M*(ftrue + b)
        // fmeas = M*ftrue + M*b

        //where:
        // M = I + Ma
        // ba = M*b = (I + Ma)*b --> b = M^-1*ba

        // We know that the norm of the true specific force is equal to the amount
        // of gravity at a certain Earth position
        // ||ftrue|| = ||g|| ~ 9.81 m/s^2

        // Hence:
        // fmeas - M*b = M*ftrue

        // M^-1 * (fmeas - M*b) = ftrue

        // ||g||^2 = ||ftrue||^2 = (M^-1 * (fmeas - M*b))^T * (M^-1 * (fmeas - M*b))
        // ||g||^2 = (fmeas - M*b)^T*(M^-1)^T * M^-1 * (fmeas - M*b)
        // ||g||^2 = (fmeas - M * b)^T * ||M^-1||^2 * (fmeas - M * b)
        // ||g||^2 = ||fmeas - M * b||^2 * ||M^-1||^2

        // Where:

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11 	m12 	m13]
        //     [m21 	m22 	m23]
        //     [m31 	m32 	m33]

        // Notice that bias b is known, hence only terms in matrix M need to be estimated

        final GradientEstimator gradientEstimator = new GradientEstimator(
                new MultiDimensionFunctionEvaluatorListener() {
                    @Override
                    public double evaluate(double[] point) throws EvaluationException {
                        return evaluateGeneral(point);
                    }
                });

        final Matrix initialM = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        initialM.add(getInitialMa());
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3871
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4283
        if (mEstimatedMg == null) {
            mEstimatedMg = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
        } else {
            mEstimatedMg.initialize(0.0);
        }

        mEstimatedMg.setElementAt(0, 0, sx);
        mEstimatedMg.setElementAt(1, 0, myx);
        mEstimatedMg.setElementAt(2, 0, mzx);

        mEstimatedMg.setElementAt(0, 1, mxy);
        mEstimatedMg.setElementAt(1, 1, sy);
        mEstimatedMg.setElementAt(2, 1, mzy);

        mEstimatedMg.setElementAt(0, 2, mxz);
        mEstimatedMg.setElementAt(1, 2, myz);
        mEstimatedMg.setElementAt(2, 2, sz);

        if (mEstimatedGg == null) {
            mEstimatedGg = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
        } else {
            mEstimatedGg.initialize(0.0);
        }

        mEstimatedGg.setElementAtIndex(0, g11);
        mEstimatedGg.setElementAtIndex(1, g21);
        mEstimatedGg.setElementAtIndex(2, g31);
        mEstimatedGg.setElementAtIndex(3, g12);
        mEstimatedGg.setElementAtIndex(4, g22);
        mEstimatedGg.setElementAtIndex(5, g32);
        mEstimatedGg.setElementAtIndex(6, g13);
        mEstimatedGg.setElementAtIndex(7, g23);
        mEstimatedGg.setElementAtIndex(8, g33);

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter.
     *
     * @throws WrongSizeException never happens.
     */
    private void setInputData() throws WrongSizeException {
        // set input data using:
        // Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + myx * Ωtruex + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
        //  fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

        final BodyKinematics expectedKinematics = new BodyKinematics();

        final int numMeasurements = mMeasurements.size();
        final Matrix x = new Matrix(numMeasurements, 2 * BodyKinematics.COMPONENTS);
        final Matrix y = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
        final double[] standardDeviations = new double[numMeasurements];
        int i = 0;
        for (final StandardDeviationFrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double omegaMeasX = measuredKinematics.getAngularRateX();
            final double omegaMeasY = measuredKinematics.getAngularRateY();
            final double omegaMeasZ = measuredKinematics.getAngularRateZ();

            final double omegaTrueX = expectedKinematics.getAngularRateX();
            final double omegaTrueY = expectedKinematics.getAngularRateY();
            final double omegaTrueZ = expectedKinematics.getAngularRateZ();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            x.setElementAt(i, 0, omegaTrueX);
            x.setElementAt(i, 1, omegaTrueY);
            x.setElementAt(i, 2, omegaTrueZ);

            x.setElementAt(i, 3, fTrueX);
            x.setElementAt(i, 4, fTrueY);
            x.setElementAt(i, 5, fTrueZ);

            y.setElementAt(i, 0, omegaMeasX);
            y.setElementAt(i, 1, omegaMeasY);
            y.setElementAt(i, 2, omegaMeasZ);

            standardDeviations[i] = measurement.getAngularRateStandardDeviation();
            i++;
        }

        mFitter.setInputData(x, y, standardDeviations);
    }

    /**
     * Converts angular speed instance to radians per second (rad/s).
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value.
     */
    private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
        return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(),
                angularSpeed.getUnit(), AngularSpeedUnit.RADIANS_PER_SECOND);
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2842
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1582
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4302
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasX() {
        return mEstimatedBiases != null ? mEstimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasY() {
        return mEstimatedBiases != null ? mEstimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasZ() {
        return mEstimatedBiases != null ? mEstimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[0],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[1],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[2],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2850
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2996
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4402
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4310
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasX() {
        return mEstimatedBiases != null ? mEstimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasY() {
        return mEstimatedBiases != null ? mEstimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasZ() {
        return mEstimatedBiases != null ? mEstimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[0],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[1],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return mEstimatedBiases != null ?
                new AngularSpeed(mEstimatedBiases[2],
                        AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7055
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1527
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2198
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFx() {
        return mEstimatedBiases != null ? mEstimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFy() {
        return mEstimatedBiases != null ? mEstimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFz() {
        return mEstimatedBiases != null ? mEstimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    public Acceleration getEstimatedBiasFxAsAcceleration() {
        return mEstimatedBiases != null ?
                new Acceleration(mEstimatedBiases[0],
                        AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[0]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    public Acceleration getEstimatedBiasFyAsAcceleration() {
        return mEstimatedBiases != null ?
                new Acceleration(mEstimatedBiases[1],
                        AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[1]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    public Acceleration getEstimatedBiasFzAsAcceleration() {
        return mEstimatedBiases != null ?
                new Acceleration(mEstimatedBiases[2],
                        AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
        if (mEstimatedBiases != null) {
            result.setValue(mEstimatedBiases[2]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1706
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1793
        mHardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMm(final Matrix initialMm)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS ||
                initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMm.getElementAtIndex(0);
        mInitialMyx = initialMm.getElementAtIndex(1);
        mInitialMzx = initialMm.getElementAtIndex(2);

        mInitialMxy = initialMm.getElementAtIndex(3);
        mInitialSy = initialMm.getElementAtIndex(4);
        mInitialMzy = initialMm.getElementAtIndex(5);

        mInitialMxz = initialMm.getElementAtIndex(6);
        mInitialMyz = initialMm.getElementAtIndex(7);
        mInitialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken.
     *
     * @return position where body magnetic flux density measurements have
     * been taken.
     */
    public NEDPosition getNedPosition() {
        return mPosition;
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken.
     *
     * @param position position where body magnetic flux density measurements
     *                 have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @return position where body magnetic flux density measurements have
     * been taken or null if not available.
     */
    public ECEFPosition getEcefPosition() {
        if (mPosition != null) {
            final ECEFPosition result = new ECEFPosition();
            getEcefPosition(result);
            return result;
        } else {
            return null;
        }
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if ECEF position could be computed, false otherwise.
     */
    public boolean getEcefPosition(final ECEFPosition result) {

        if (mPosition != null) {
            final ECEFVelocity velocity = new ECEFVelocity();
            NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                    mPosition.getLatitude(), mPosition.getLongitude(),
                    mPosition.getHeight(), 0.0, 0.0, 0.0,
                    result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param position position where body magnetic flux density have been
     *                 taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Gets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @return timestamp expressed as decimal year or null if not defined.
     */
    public Double getYear() {
        return mYear;
    }

    /**
     * Sets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @param year timestamp expressed as decimal year.
     * @throws LockedException if calibrator is currently running.
     */
    public void setYear(final Double year) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = year;
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param timestampMillis a timestamp expressed in milliseocnds since
     *                        epoch time (January 1st, 1970 at midnight).
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Long timestampMillis) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(timestampMillis);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param date a date instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Date date) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(date);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param calendar a calendar instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final GregorianCalendar calendar)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(calendar);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 1722
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1777
        mInitialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMm(final Matrix initialMm)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS ||
                initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMm.getElementAtIndex(0);
        mInitialMyx = initialMm.getElementAtIndex(1);
        mInitialMzx = initialMm.getElementAtIndex(2);

        mInitialMxy = initialMm.getElementAtIndex(3);
        mInitialSy = initialMm.getElementAtIndex(4);
        mInitialMzy = initialMm.getElementAtIndex(5);

        mInitialMxz = initialMm.getElementAtIndex(6);
        mInitialMyz = initialMm.getElementAtIndex(7);
        mInitialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken.
     *
     * @return position where body magnetic flux density measurements have
     * been taken.
     */
    public NEDPosition getNedPosition() {
        return mPosition;
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken.
     *
     * @param position position where body magnetic flux density measurements
     *                 have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @return position where body magnetic flux density measurements have
     * been taken or null if not available.
     */
    public ECEFPosition getEcefPosition() {
        if (mPosition != null) {
            final ECEFPosition result = new ECEFPosition();
            getEcefPosition(result);
            return result;
        } else {
            return null;
        }
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if ECEF position could be computed, false otherwise.
     */
    public boolean getEcefPosition(final ECEFPosition result) {

        if (mPosition != null) {
            final ECEFVelocity velocity = new ECEFVelocity();
            NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                    mPosition.getLatitude(), mPosition.getLongitude(),
                    mPosition.getHeight(), 0.0, 0.0, 0.0,
                    result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param position position where body magnetic flux density have been
     *                 taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Gets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @return timestamp expressed as decimal year or null if not defined.
     */
    public Double getYear() {
        return mYear;
    }

    /**
     * Sets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @param year timestamp expressed as decimal year.
     * @throws LockedException if calibrator is currently running.
     */
    public void setYear(final Double year) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = year;
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param timestampMillis a timestamp expressed in milliseocnds since
     *                        epoch time (January 1st, 1970 at midnight).
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Long timestampMillis) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(timestampMillis);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param date a date instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Date date) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(date);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param calendar a calendar instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final GregorianCalendar calendar)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mYear = convertTime(calendar);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2084
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2351
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2558
        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containint coordinates of known bias.
     */
    @Override
    public double[] getBias() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2085
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2352
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2559
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3042
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3007
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containint coordinates of known bias.
     */
    @Override
    public double[] getBias() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2746
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4139
                && mSequences.size() >= getMinimumRequiredSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public abstract void calibrate() throws LockedException, NotReadyException,
            CalibrationException;

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredSequences()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredSequences}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than
     *                                  {@link #getMinimumRequiredSequences}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredSequences()) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2085
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2352
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2185
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2559
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3042
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3007
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containint coordinates of known bias.
     */
    @Override
    public double[] getBias() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2113
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2221
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
        return mEstimatedChiSq;
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter.
     *
     * @throws WrongSizeException never happens.
     * @throws IOException        if world magnetic model cannot be loaded.
     */
    private void setInputData() throws WrongSizeException, IOException {

        final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
        if (mMagneticModel != null) {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(mMagneticModel);
        } else {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
        }

        final NEDPosition position = getNedPosition();
        final NEDMagneticFluxDensity earthB = wmmEstimator.estimate(
                position, mYear);
        final double b = earthB.getNorm();
        final double b2 = b * b;

        final int numMeasurements = mMeasurements.size();
        final Matrix x = new Matrix(numMeasurements,
                BodyMagneticFluxDensity.COMPONENTS);
        final double[] y = new double[numMeasurements];
        final double[] specificForceStandardDeviations = new double[numMeasurements];
        int i = 0;
        for (final StandardDeviationBodyMagneticFluxDensity measurement : mMeasurements) {
            final BodyMagneticFluxDensity measuredMagneticFluxDensity =
                    measurement.getMagneticFluxDensity();

            final double bmeasX = measuredMagneticFluxDensity.getBx();
            final double bmeasY = measuredMagneticFluxDensity.getBy();
            final double bmeasZ = measuredMagneticFluxDensity.getBz();

            x.setElementAt(i, 0, bmeasX);
            x.setElementAt(i, 1, bmeasY);
            x.setElementAt(i, 2, bmeasZ);

            y[i] = b2;

            specificForceStandardDeviations[i] =
                    measurement.getMagneticFluxDensityStandardDeviation();

            i++;
        }

        mFitter.setInputData(x, y, specificForceStandardDeviations);
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws FittingException                         if Levenberg-Marquardt fails for numerical reasons.
     * @throws AlgebraException                         if there are numerical instabilities that prevent
     *                                                  matrix inversion.
     * @throws com.irurueta.numerical.NotReadyException never happens.
     * @throws IOException                              if world magnetic model cannot be loaded.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException, IOException {
        // The magnetometer model is:
        // bmeas = bm + (I + Mm) * btrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // bmeas = bm + (I + Mm) * btrue

        // For convergence purposes of the Levenberg-Marquardt algorithm, the
        // magnetometer model can be better expressed as:
        // bmeas = T*K*(btrue + b)
        // bmeas = M*(btrue + b)
        // bmeas = M*btrue + M*b

        // where:
        // M = I + Mm
        // bm = M*b = (I + Mm)*b --> b = M^-1*bm

        // We know that the norm of the true body magnetic flux density
        // is equal to the amount of Earth magnetic flux density at provided
        // position and timestamp
        // ||btrue|| = ||bEarth|| --> from 30 µT to 60 µT

        // Hence:
        // bmeas - M*b = M*btrue

        // M^-1 * (bmeas - M*b) = btrue

        // ||bEarth||^2 = ||btrue||^2 = (M^-1 * (bmeas - M*b))^T * (M^-1 * (bmeas - M*b))
        // ||bEarth||^2 = (bmeas - M*b)^T*(M^-1)^T * M^-1 * (bmeas - M*b)
        // ||bEarth||^2 = (bmeas - M * b)^T * ||M^-1||^2 * (bmeas - M * b)
        // ||bEarth||^2 = ||bmeas - M * b||^2 * ||M^-1||^2

        // Where:

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11 	m12 	m13]
        //     [m21 	m22 	m23]
        //     [m31 	m32 	m33]

        final GradientEstimator gradientEstimator = new GradientEstimator(
                new MultiDimensionFunctionEvaluatorListener() {
                    @Override
                    public double evaluate(final double[] point)
                            throws EvaluationException {
                        return evaluateGeneral(point);
                    }
                });

        final Matrix initialM = Matrix.identity(BodyMagneticFluxDensity.COMPONENTS,
                BodyMagneticFluxDensity.COMPONENTS);
        initialM.add(getInitialMm());
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 706
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1798
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for the accelerometer, gyroscope and magnetometer.
     *
     * @throws AlgebraException if there are numerical errors.
     * @throws IOException      if world magnetic model cannot be loaded.
     */
    private void calibrateCommonAxis() throws AlgebraException, IOException {
        // The magnetometer model is:
        // mBmeas = bm + (I + Mm) * mBtrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // mBmeas = bm + (I + Mm) * mBtrue

        // Hence:
        //  [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        //  [mBmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [mBtruey]
        //  [mBmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [mBtruez]

        // where myx = mzx = mzy = 0

        // Hence:
        //  [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        //  [mBmeasy] = [by]     [0  1   0]   [0     sy  myz]    [mBtruey]
        //  [mBmeasz] = [bz]     [0  0   1]   [0     0   sz ]    [mBtruez]

        //  [mBmeasx] = [bx] +   [1+sx   mxy     mxz ][mBtruex]
        //  [mBmeasy]   [by]     [0      1+sy    myz ][mBtruey]
        //  [mBmeasz]   [bz]     [0      0       1+sz][mBtruez]

        //  mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy = by + (1+sy) * mBtruey + myz * mBtruez
        //  mBmeasz = bz + (1+sz) * mBtruez

        // Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myz
        // Reordering:
        //  mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy = by + mBtruey + sy * mBtruey + myz * mBtruez
        //  mBmeasz = bz + mBtruez + sz * mBtruez

        //  mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy - mBtruey = by + sy * mBtruey + myz * mBtruez
        //  mBmeasz - mBtruez = bz + sz * mBtruez

        // [1   0   0   mBtruex  0        0        mBtruey  mBtruez  0      ][bx ] = [mBmeasx - mBtruex]
        // [0   1   0   0        mBtruey  0        0        0        mBtruez][by ]   [mBmeasy - mBtruey]
        // [0   0   1   0        0        mBtruez  0        0        0      ][bz ]   [mBmeasz - mBtruez]
        //                                                                   [sx ]
        //                                                                   [sy ]
        //                                                                   [sz ]
        //                                                                   [mxy]
        //                                                                   [mxz]
        //                                                                   [myz]

        final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
        if (mMagneticModel != null) {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(mMagneticModel);
        } else {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
        }

        final BodyMagneticFluxDensity expectedMagneticFluxDensity =
                new BodyMagneticFluxDensity();
        final NEDFrame nedFrame = new NEDFrame();
        final NEDMagneticFluxDensity earthB = new NEDMagneticFluxDensity();
        final CoordinateTransformation cbn = new CoordinateTransformation(
                FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final CoordinateTransformation cnb = new CoordinateTransformation(
                FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);

        final int rows = EQUATIONS_PER_MEASUREMENT * mMeasurements.size();
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyMagneticFluxDensity measurement : mMeasurements) {
            final BodyMagneticFluxDensity measuredMagneticFluxDensity =
                    measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final ECEFFrame ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final double year = measurement.getYear();

            final double latitude = nedFrame.getLatitude();
            final double longitude = nedFrame.getLongitude();
            final double height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body mangetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb,
                    expectedMagneticFluxDensity);

            final double bMeasX = measuredMagneticFluxDensity.getBx();
            final double bMeasY = measuredMagneticFluxDensity.getBy();
            final double bMeasZ = measuredMagneticFluxDensity.getBz();

            final double bTrueX = expectedMagneticFluxDensity.getBx();
            final double bTrueY = expectedMagneticFluxDensity.getBy();
            final double bTrueZ = expectedMagneticFluxDensity.getBz();

            a.setElementAt(i, 0, 1.0);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4477
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4254
        if (mEstimatedMm == null) {
            mEstimatedMm = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
        } else {
            mEstimatedMm.initialize(0.0);
        }

        mEstimatedMm.setElementAt(0, 0, sx);
        mEstimatedMm.setElementAt(1, 0, myx);
        mEstimatedMm.setElementAt(2, 0, mzx);

        mEstimatedMm.setElementAt(0, 1, mxy);
        mEstimatedMm.setElementAt(1, 1, sy);
        mEstimatedMm.setElementAt(2, 1, mzy);

        mEstimatedMm.setElementAt(0, 2, mxz);
        mEstimatedMm.setElementAt(1, 2, myz);
        mEstimatedMm.setElementAt(2, 2, sz);

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter.
     *
     * @throws WrongSizeException never happens.
     * @throws IOException        if world magnetic model cannot be loaded.
     */
    private void setInputData() throws WrongSizeException, IOException {
        // set input data using:
        //  mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy = by + myx * mBtruex + mBtruey + sy * mBtruey + myz * mBtruez
        //  mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + mBtruez + sz * mBtruez

        final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
        if (mMagneticModel != null) {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(mMagneticModel);
        } else {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
        }

        final BodyMagneticFluxDensity expectedMagneticFluxDensity = new BodyMagneticFluxDensity();
        final NEDFrame nedFrame = new NEDFrame();
        final NEDMagneticFluxDensity earthB = new NEDMagneticFluxDensity();
        final CoordinateTransformation cbn = new CoordinateTransformation(
                FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final CoordinateTransformation cnb = new CoordinateTransformation(
                FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);

        final int numMeasurements = mMeasurements.size();
        final Matrix x = new Matrix(numMeasurements,
                BodyMagneticFluxDensity.COMPONENTS);
        final Matrix y = new Matrix(numMeasurements,
                BodyMagneticFluxDensity.COMPONENTS);
        final double[] specificForceStandardDeviations = new double[
                numMeasurements];
        int i = 0;
        for (final StandardDeviationFrameBodyMagneticFluxDensity measurement : mMeasurements) {
            final BodyMagneticFluxDensity measuredMagneticFluxDensity =
                    measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final ECEFFrame ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final double year = measurement.getYear();

            final double latitude = nedFrame.getLatitude();
            final double longitude = nedFrame.getLongitude();
            final double height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body mangetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb,
                    expectedMagneticFluxDensity);

            final double bMeasX = measuredMagneticFluxDensity.getBx();
            final double bMeasY = measuredMagneticFluxDensity.getBy();
            final double bMeasZ = measuredMagneticFluxDensity.getBz();

            final double bTrueX = expectedMagneticFluxDensity.getBx();
            final double bTrueY = expectedMagneticFluxDensity.getBy();
            final double bTrueZ = expectedMagneticFluxDensity.getBz();

            x.setElementAt(i, 0, bTrueX);
            x.setElementAt(i, 1, bTrueY);
            x.setElementAt(i, 2, bTrueZ);

            y.setElementAt(i, 0, bMeasX);
            y.setElementAt(i, 1, bMeasY);
            y.setElementAt(i, 2, bMeasZ);

            specificForceStandardDeviations[i] =
                    measurement.getMagneticFluxDensityStandardDeviation();
            i++;
        }

        mFitter.setInputData(x, y, specificForceStandardDeviations);
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2511
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2469
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2513
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2475
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
        return mSequences;
    }

    /**
     * Sets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @param sequences collection of sequences of timestamped body
     *                  kinematics measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setSequences(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mSequences = sequences;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Indicates whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @return true if G-dependent cross biases will be estimated,
     * false otherwise.
     */
    public boolean isGDependentCrossBiasesEstimated() {
        return mEstimateGDependentCrossBiases;
    }

    /**
     * Specifies whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setGDependentCrossBiasesEstimated(
            final boolean estimateGDependentCrossBiases)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mEstimateGDependentCrossBiases = estimateGDependentCrossBiases;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public EasyGyroscopeCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6317
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6757
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS ||
                initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMa.getElementAtIndex(0);
        mInitialMyx = initialMa.getElementAtIndex(1);
        mInitialMzx = initialMa.getElementAtIndex(2);

        mInitialMxy = initialMa.getElementAtIndex(3);
        mInitialSy = initialMa.getElementAtIndex(4);
        mInitialMzy = initialMa.getElementAtIndex(5);

        mInitialMxz = initialMa.getElementAtIndex(6);
        mInitialMyz = initialMa.getElementAtIndex(7);
        mInitialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return mPosition;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final NEDPosition result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (mPosition != null) {
            final NEDVelocity velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    mPosition.getX(), mPosition.getY(), mPosition.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @param measurements collection of body kinematics measurements at a
     *                     known position witn unknown orientations.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMeasurements(
            final Collection<StandardDeviationBodyKinematics> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public KnownBiasAndPositionAccelerometerCalibrationListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1719
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1761
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS ||
                initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMa.getElementAtIndex(0);
        mInitialMyx = initialMa.getElementAtIndex(1);
        mInitialMzx = initialMa.getElementAtIndex(2);

        mInitialMxy = initialMa.getElementAtIndex(3);
        mInitialSy = initialMa.getElementAtIndex(4);
        mInitialMzy = initialMa.getElementAtIndex(5);

        mInitialMxz = initialMa.getElementAtIndex(6);
        mInitialMyz = initialMa.getElementAtIndex(7);
        mInitialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return mPosition;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final NEDPosition result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (mPosition != null) {
            final NEDVelocity velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    mPosition.getX(), mPosition.getY(), mPosition.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Gets a list of body kinematics measurements taken at a
     * given position with different unknown orientations and containing the
     * standard deviations of accelerometer and gyroscope measurements.
     *
     * @return list of body kinematics measurements.
     */
    public List<StandardDeviationBodyKinematics> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets a list of body kinematics measurements taken at a
     * given position with different unknown orientations and containing the
     * standard deviations of accelerometer and gyroscope measurements.
     *
     * @param measurements list of body kinematics measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMeasurements(
            final List<StandardDeviationBodyKinematics> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public RobustKnownBiasAndPositionAccelerometerCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1866
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1977
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(
            final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(
            final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1240
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2141
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanInitializerConfig.java 112
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanInitializerConfig.java 138
    public INSLooselyCoupledKalmanInitializerConfig(final INSLooselyCoupledKalmanInitializerConfig input) {
        copyFrom(input);
    }

    /**
     * Gets initial attitude uncertainty per axis expressed in radians (rad).
     *
     * @return initial attitude uncertainty per axis expressed in radians (rad).
     */
    public double getInitialAttitudeUncertainty() {
        return mInitialAttitudeUncertainty;
    }

    /**
     * Sets initial attitude uncertainty per axis expressed in radians (rad).
     *
     * @param initialAttitudeUncertainty initial attitude uncertainty per axis expressed
     *                                   in radians (rad).
     */
    public void setInitialAttitudeUncertainty(final double initialAttitudeUncertainty) {
        mInitialAttitudeUncertainty = initialAttitudeUncertainty;
    }

    /**
     * Gets initial attitude uncertainty per axis.
     *
     * @param result instance where initial attitude uncertainty per axis will be stored.
     */
    public void getInitialAttitudeUncertaintyAngle(final Angle result) {
        result.setValue(mInitialAttitudeUncertainty);
        result.setUnit(AngleUnit.RADIANS);
    }

    /**
     * Gets initial attitude uncertainty per axis.
     *
     * @return initial attitude uncertainty per axis.
     */
    public Angle getInitialAttitudeUncertaintyAngle() {
        return new Angle(mInitialAttitudeUncertainty, AngleUnit.RADIANS);
    }

    /**
     * Sets initial attitude uncertainty per axis.
     *
     * @param initialAttitudeUncertainty initial attitude uncertainty per axis.
     */
    public void setInitialAttitudeUncertainty(final Angle initialAttitudeUncertainty) {
        mInitialAttitudeUncertainty = AngleConverter.convert(
                initialAttitudeUncertainty.getValue().doubleValue(),
                initialAttitudeUncertainty.getUnit(), AngleUnit.RADIANS);
    }

    /**
     * Gets initial velocity uncertainty per axis expressed in meters per second (m/s).
     *
     * @return initial velocity uncertainty per axis expressed in meters per second (m/s).
     */
    public double getInitialVelocityUncertainty() {
        return mInitialVelocityUncertainty;
    }

    /**
     * Sets initial velocity uncertainty per axis expressed in meters per second (m/s).
     *
     * @param initialVelocityUncertainty initial velocity uncertainty per axis expressed
     *                                   in meters per second (m/s).
     */
    public void setInitialVelocityUncertainty(final double initialVelocityUncertainty) {
        mInitialVelocityUncertainty = initialVelocityUncertainty;
    }

    /**
     * Gets initial velocity uncertainty per axis.
     *
     * @param result instance where initial attitude uncertainty per axis will be stored.
     */
    public void getInitialVelocityUncertaintySpeed(final Speed result) {
        result.setValue(mInitialVelocityUncertainty);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets initial velocity uncertainty per axis.
     *
     * @return initial velocity uncertainty per axis.
     */
    public Speed getInitialVelocityUncertaintySpeed() {
        return new Speed(mInitialVelocityUncertainty, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets initial velocity uncertainty per axis.
     *
     * @param initialVelocityUncertainty initial velocity uncertainty per axis.
     */
    public void setInitialVelocityUncertainty(final Speed initialVelocityUncertainty) {
        mInitialVelocityUncertainty = SpeedConverter.convert(
                initialVelocityUncertainty.getValue().doubleValue(),
                initialVelocityUncertainty.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets initial position uncertainty per axis expressed in meters (m)
     *
     * @return initial position uncertainty per axis expressed in meters (m).
     */
    public double getInitialPositionUncertainty() {
        return mInitialPositionUncertainty;
    }

    /**
     * Sets initial position uncertainty per axis expressed in meters (m)
     *
     * @param initialPositionUncertainty initial position uncertainty per axis expressed
     *                                   in meters (m).
     */
    public void setInitialPositionUncertainty(final double initialPositionUncertainty) {
        mInitialPositionUncertainty = initialPositionUncertainty;
    }

    /**
     * Gets initial position uncertainty per axis.
     *
     * @param result instance where initial position uncertainty per axis will be stored.
     */
    public void getInitialPositionUncertaintyDistance(final Distance result) {
        result.setValue(mInitialPositionUncertainty);
        result.setUnit(DistanceUnit.METER);
    }

    /**
     * Gets initial position uncertainty per axis.
     *
     * @return initial position uncertainty per axis.
     */
    public Distance getInitialPositionUncertaintyDistance() {
        return new Distance(mInitialPositionUncertainty, DistanceUnit.METER);
    }

    /**
     * Sets initial position uncertainty per axis.
     *
     * @param initialPositionUncertainty initial position uncertainty per axis.
     */
    public void setInitialPositionUncertainty(final Distance initialPositionUncertainty) {
        mInitialPositionUncertainty = DistanceConverter.convert(
                initialPositionUncertainty.getValue().doubleValue(),
                initialPositionUncertainty.getUnit(), DistanceUnit.METER);
    }

    /**
     * Gets initial acceleration bias uncertainty expressed in meters per squared second (m/s^2).
     *
     * @return initial acceleration bias uncertainty expressed in meters per squared second (m/s^2).
     */
    public double getInitialAccelerationBiasUncertainty() {
        return mInitialAccelerationBiasUncertainty;
    }

    /**
     * Sets initial acceleration bias uncertainty expressed in meters per squared second (m/s^2).
     *
     * @param initialAccelerationBiasUncertainty initial acceleration bias uncertainty expressed in
     *                                           meters per squared second (m/s^2).
     */
    public void setInitialAccelerationBiasUncertainty(
            final double initialAccelerationBiasUncertainty) {
        mInitialAccelerationBiasUncertainty = initialAccelerationBiasUncertainty;
    }

    /**
     * Gets initial acceleration bias uncertainty.
     *
     * @param result instance where initial acceleration bias uncertainty will be stored.
     */
    public void getInitialAccelerationBiasUncertaintyAcceleration(final Acceleration result) {
        result.setValue(mInitialAccelerationBiasUncertainty);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial acceleration bias uncertainty.
     *
     * @return initial acceleration bias uncertainty.
     */
    public Acceleration getInitialAccelerationBiasUncertaintyAcceleration() {
        return new Acceleration(mInitialAccelerationBiasUncertainty,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial acceleration bias uncertainty.
     *
     * @param initialAccelerationUncertainty initial acceleration bias uncertainty.
     */
    public void setInitialAccelerationBiasUncertainty(
            final Acceleration initialAccelerationUncertainty) {
        mInitialAccelerationBiasUncertainty = AccelerationConverter.convert(
                initialAccelerationUncertainty.getValue().doubleValue(),
                initialAccelerationUncertainty.getUnit(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial gyroscope bias uncertainty expressed in radians per second (rad/s).
     *
     * @return initial gyroscope bias uncertainty expressed in radians per second (rad/s).
     */
    public double getInitialGyroscopeBiasUncertainty() {
        return mInitialGyroscopeBiasUncertainty;
    }

    /**
     * Sets initial gyroscope bias uncertainty expressed in radians per second (rad/s).
     *
     * @param initialGyroscopeBiasUncertainty initial gyroscope bias uncertainty expressed
     *                                        in radians per second (rad/s).
     */
    public void setInitialGyroscopeBiasUncertainty(
            final double initialGyroscopeBiasUncertainty) {
        mInitialGyroscopeBiasUncertainty = initialGyroscopeBiasUncertainty;
    }

    /**
     * Gets initial gyroscope bias uncertainty.
     *
     * @param result instance where initial gyroscope bias uncertainty will be stored.
     */
    public void getInitialGyroscopeBiasUncertaintyAngularSpeed(
            final AngularSpeed result) {
        result.setValue(mInitialGyroscopeBiasUncertainty);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial gyroscope bias uncertainty.
     *
     * @return initial gyroscope bias uncertainty.
     */
    public AngularSpeed getInitialGyroscopeBiasUncertaintyAngularSpeed() {
        return new AngularSpeed(mInitialGyroscopeBiasUncertainty,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial gyroscope bias uncertainty.
     *
     * @param initialGyroscopeBiasUncertainty initial gyroscope bias uncertainty.
     */
    public void setInitialGyroscopeBiasUncertainty(
            final AngularSpeed initialGyroscopeBiasUncertainty) {
        mInitialGyroscopeBiasUncertainty = AngularSpeedConverter.convert(
                initialGyroscopeBiasUncertainty.getValue().doubleValue(),
                initialGyroscopeBiasUncertainty.getUnit(),
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets configuration parameters.
     *
     * @param initialAttitudeUncertainty         initial attitude uncertainty per axis
     *                                           expressed in radians (rad).
     * @param initialVelocityUncertainty         initial velocity uncertainty per axis
     *                                           expressed in meters per second (m/s).
     * @param initialPositionUncertainty         initial position uncertainty per axis
     *                                           expressed in meters (m).
     * @param initialAccelerationBiasUncertainty initial acceleration bias uncertainty
     *                                           expressed in meters per squared second (m/s^2).
     * @param initialGyroscopeBiasUncertainty    initial gyroscope bias uncertainty
     *                                           expressed in radians per second (rad/s).
     */
    public void setValues(final double initialAttitudeUncertainty,
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2136
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1162
                                          final ECEFFrame frame, final ECEFFrame oldFrame,
                                          final BodyKinematics result) {
        estimateKinematics(timeInterval, frame,
                oldFrame.getCoordinateTransformation(),
                oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final double timeInterval,
                                          final CoordinateTransformation c,
                                          final CoordinateTransformation oldC,
                                          final Speed vx, final Speed vy, final Speed vz,
                                          final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                          final double x, final double y, final double z,
                                          final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC,
                SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final Time timeInterval,
                                          final CoordinateTransformation c,
                                          final CoordinateTransformation oldC,
                                          final Speed vx, final Speed vy, final Speed vz,
                                          final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                          final double x, final double y, final double z,
                                          final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC,
                SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param position     cartesian body position resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final double timeInterval,
                                          final CoordinateTransformation c,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 5944
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6384
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1327
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 689
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1361
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2120
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2083
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3317
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2122
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2089
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3275
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 709
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3328
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3373
        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    public double[] getBias() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 5945
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6385
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1328
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 690
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1362
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2121
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2084
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3318
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2123
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2090
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3276
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 710
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3329
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3374
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1308
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 1320
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 588
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 570
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1379
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1391
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    public double[] getBias() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2475
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2958
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<? extends StandardDeviationFrameBodyKinematics> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body kinematics measurements taken at different
     *                     frames (positions, orientations and velocities).
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(
            final Collection<? extends StandardDeviationFrameBodyKinematics> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mg matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mg matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    @Override
    public KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 5944
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6384
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1319
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1327
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 689
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1361
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2120
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2083
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3317
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2122
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1347
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2089
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3275
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 709
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3328
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3373
        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    public double[] getBias() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1320
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1348
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1308
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 1320
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 588
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 570
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1379
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1391
    }

    /**
     * Gets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x scaling factor.
     */
    public double getInitialSx() {
        return mInitialSx;
    }

    /**
     * Sets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSx(final double initialSx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y scaling factor.
     */
    public double getInitialSy() {
        return mInitialSy;
    }

    /**
     * Sets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSy(final double initialSy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z scaling factor.
     */
    public double getInitialSz() {
        return mInitialSz;
    }

    /**
     * Sets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialSz(final double initialSz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-y cross coupling error.
     */
    public double getInitialMxy() {
        return mInitialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-z cross coupling error.
     */
    public double getInitialMxz() {
        return mInitialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-x cross coupling error.
     */
    public double getInitialMyx() {
        return mInitialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-z cross coupling error.
     */
    public double getInitialMyz() {
        return mInitialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-x cross coupling error.
     */
    public double getInitialMzx() {
        return mInitialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-y cross coupling error.
     */
    public double getInitialMzy() {
        return mInitialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialSx = initialSx;
        mInitialSy = initialSy;
        mInitialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialMxy = initialMxy;
        mInitialMxz = initialMxz;
        mInitialMyx = initialMyx;
        mInitialMyz = initialMyz;
        mInitialMzx = initialMzx;
        mInitialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanEpochEstimator.java 688
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanEpochEstimator.java 104
                INSLooselyCoupledKalmanState.NUM_PARAMS);

        final Matrix tmp1 = omegaIe.multiplyByScalarAndReturnNew(
                propagationInterval);
        final Matrix tmp2 = phiMatrix.getSubmatrix(0, 0,
                2, 2);
        tmp2.subtract(tmp1);

        phiMatrix.setSubmatrix(0, 0,
                2, 2, tmp2);

        final Matrix estCbeOld = previousState
                .getBodyToEcefCoordinateTransformationMatrix();
        tmp1.copyFrom(estCbeOld);
        tmp1.multiplyByScalar(propagationInterval);

        phiMatrix.setSubmatrix(0, 12,
                2, 14, tmp1);
        phiMatrix.setSubmatrix(3, 9,
                5, 11, tmp1);

        final Matrix measFibb = new Matrix(BodyKinematics.COMPONENTS, 1);
        measFibb.setElementAtIndex(0, fx);
        measFibb.setElementAtIndex(1, fy);
        measFibb.setElementAtIndex(2, fz);

        estCbeOld.multiply(measFibb, tmp1);

        Utils.skewMatrix(tmp1, tmp2);
        tmp2.multiplyByScalar(-propagationInterval);

        phiMatrix.setSubmatrix(3, 0,
                5, 2, tmp2);

        phiMatrix.getSubmatrix(3, 3,
                5, 5, tmp1);
        tmp2.copyFrom(omegaIe);
        tmp2.multiplyByScalar(2.0 * propagationInterval);
        tmp1.subtract(tmp2);
        phiMatrix.setSubmatrix(3, 3,
                5, 5, tmp1);

        final double sinPrevLat = Math.sin(previousLatitude);
        final double cosPrevLat = Math.cos(previousLatitude);
        final double sinPrevLat2 = sinPrevLat * sinPrevLat;
        final double cosPrevLat2 = cosPrevLat * cosPrevLat;

        // From (2.137)
        final double geocentricRadius = EARTH_EQUATORIAL_RADIUS_WGS84
                / Math.sqrt(1.0 - Math.pow(EARTH_ECCENTRICITY * sinPrevLat, 2.0))
                * Math.sqrt(cosPrevLat2
                + Math.pow(1.0 - EARTH_ECCENTRICITY * EARTH_ECCENTRICITY, 2.0) * sinPrevLat2);

        final double prevX = previousState.getX();
        final double prevY = previousState.getY();
        final double prevZ = previousState.getZ();
        final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
                prevX, prevY, prevZ);

        final double previousPositionNorm = Math.sqrt(prevX * prevX +
                prevY * prevY + prevZ * prevZ);

        final Matrix estRebeOld = new Matrix(ECEFPosition.COMPONENTS, 1);
        estRebeOld.setElementAtIndex(0, prevX);
        estRebeOld.setElementAtIndex(1, prevY);
        estRebeOld.setElementAtIndex(2, prevZ);

        final Matrix g = gravity.asMatrix();
        g.multiplyByScalar(-2.0 * propagationInterval / geocentricRadius);

        final Matrix estRebeOldTrans = estRebeOld.transposeAndReturnNew();
        estRebeOldTrans.multiplyByScalar(1.0 / previousPositionNorm);

        g.multiply(estRebeOldTrans, tmp1);

        phiMatrix.setSubmatrix(3, 6,
                5, 8, tmp1);

        for (int i = 0; i < ECEFPosition.COMPONENTS; i++) {
            phiMatrix.setElementAt(6 + i, 3 + i, propagationInterval);
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1259
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1318
            final RobustKnownFrameAccelerometerCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return mUseLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mUseLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return mRefinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(
            final boolean preliminarySolutionRefined) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mRefinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(
            final float progressDelta) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(
            final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(
            final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(
            final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(
            final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1638
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1103
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    public List<StandardDeviationFrameBodyKinematics> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body kinematics measurements taken at different
     *                     frames (positions, orientations and velocities).
     * @throws LockedException if calibrator is currently running.
     */
    public void setMeasurements(
            final List<StandardDeviationFrameBodyKinematics> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCommonAxisUsed(final boolean commonAxisUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public RobustKnownBiasAndFrameGyroscopeCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 918
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 925
        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containint coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mBiasX;
        result[1] = mBiasY;
        result[2] = mBiasZ;
    }

    /**
     * Sets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mBiasX = bias[0];
        mBiasY = bias[1];
        mBiasZ = bias[2];
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @return known accelerometer bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mBiasX);
        result.setElementAtIndex(1, mBiasY);
        result.setElementAtIndex(2, mBiasZ);
    }

    /**
     * Sets known accelerometer bias as a column matrix.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS
                || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mBiasX = bias.getElementAtIndex(0);
        mBiasY = bias.getElementAtIndex(1);
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Indicates whether estimator is ready to start the estimator.
     *
     * @return true if estimator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null
                && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is currently running or not.
     *
     * @return true if estimator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if estimator is currently running.
     * @throws NotReadyException    if estimator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/indoor/Utils.java 503
com/irurueta/navigation/indoor/Utils.java 1203
    public static MultivariateNormalDist propagateVariancesToRssiVarianceFirstOrderNonLinear3D(
            final double fingerprintRssi, final double pathLossExponent,
            final Point3D fingerprintPosition, final Point3D radioSourcePosition,
            final Point3D estimatedPosition,
            final Double fingerprintRssiVariance,
            final Double pathLossExponentVariance,
            final Matrix fingerprintPositionCovariance,
            final Matrix radioSourcePositionCovariance,
            final Matrix estimatedPositionCovariance) throws IndoorException {

        if (fingerprintPosition == null || radioSourcePosition == null ||
                estimatedPosition == null) {
            return null;
        }

        //1st order Taylor expression of received power in 3D:
        //Pr(pi) = Pr(p1)
        //  - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
        //  - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
        //  - 10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1)
        //where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2

        final double x1 = fingerprintPosition.getInhomX();
        final double y1 = fingerprintPosition.getInhomY();
        final double z1 = fingerprintPosition.getInhomZ();

        final double xa = radioSourcePosition.getInhomX();
        final double ya = radioSourcePosition.getInhomY();
        final double za = radioSourcePosition.getInhomZ();

        final double xi = estimatedPosition.getInhomX();
        final double yi = estimatedPosition.getInhomY();
        final double zi = estimatedPosition.getInhomZ();

        final double[] mean = new double[]{
                fingerprintRssi, pathLossExponent, x1, y1, z1, xa, ya, za, xi, yi, zi
        };
        final Matrix covariance = Matrix.diagonal(new double[]{
                fingerprintRssiVariance != null ? fingerprintRssiVariance : 0.0,
                pathLossExponentVariance != null ? pathLossExponentVariance : 0.0,
                0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
        });

        if (fingerprintPositionCovariance != null &&
                fingerprintPositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH &&
                fingerprintPositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {

            covariance.setSubmatrix(2, 2,
                    4, 4,
                    fingerprintPositionCovariance);
        }

        if (radioSourcePositionCovariance != null &&
                radioSourcePositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH &&
                radioSourcePositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
            covariance.setSubmatrix(5, 5,
                    7, 7,
                    radioSourcePositionCovariance);
        }

        if (estimatedPositionCovariance != null &&
                estimatedPositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH &&
                estimatedPositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
            covariance.setSubmatrix(8, 8,
                    10, 10,
                    estimatedPositionCovariance);
        }

        try {
            return MultivariateNormalDist.propagate(new MultivariateNormalDist.JacobianEvaluator() {
                @Override
                public void evaluate(
                        final double[] x, final double[] y, final Matrix jacobian) {

                    //Pr(pi) = Pr(p1)
                    //  - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
                    //  - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
                    //  - 10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1)
                    //where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2

                    //Hence:
                    //Pr(pi) = Pr(p1) -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/
                    //      (ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))

                    final double diffX1a = x1 - xa;
                    final double diffY1a = y1 - ya;
                    final double diffZ1a = z1 - za;

                    final double diffXi1 = xi - x1;
                    final double diffYi1 = yi - y1;
                    final double diffZi1 = zi - z1;

                    final double diffX1a2 = diffX1a * diffX1a;
                    final double diffY1a2 = diffY1a * diffY1a;
                    final double diffZ1a2 = diffZ1a * diffZ1a;

                    final double d1a2 = diffX1a2 + diffY1a2 + diffZ1a2;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 918
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2936
        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containint coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mBiasX;
        result[1] = mBiasY;
        result[2] = mBiasZ;
    }

    /**
     * Sets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mBiasX = bias[0];
        mBiasY = bias[1];
        mBiasZ = bias[2];
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @return known accelerometer bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mBiasX);
        result.setElementAtIndex(1, mBiasY);
        result.setElementAtIndex(2, mBiasZ);
    }

    /**
     * Sets known accelerometer bias as a column matrix.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS
                || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mBiasX = bias.getElementAtIndex(0);
        mBiasY = bias.getElementAtIndex(1);
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Indicates whether estimator is ready to start the estimator.
     *
     * @return true if estimator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null
                && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is currently running or not.
     *
     * @return true if estimator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if estimator is currently running.
     * @throws NotReadyException    if estimator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 688
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 1860
        mListener = listener;
    }

    /**
     * Gets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return mBiasX;
    }

    /**
     * Sets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasX x coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
    }

    /**
     * Gets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return y coordinate of accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return mBiasY;
    }

    /**
     * Sets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasY y coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasY = biasY;
    }

    /**
     * Gets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return z coordinate of accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return mBiasZ;
    }

    /**
     * Sets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasZ = biasZ;
    }

    /**
     * Gets known x coordinate of accelerometer bias.
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
        return new Acceleration(mBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x coordinate of accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x coordinate of accelerometer bias.
     *
     * @param biasX x coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasX(final Acceleration biasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasX = convertAcceleration(biasX);
    }

    /**
     * Gets known y coordinate of accelerometer bias.
     *
     * @return y coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasYAsAcceleration() {
        return new Acceleration(mBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y coordinate of accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y coordinate of accelerometer bias.
     *
     * @param biasY y coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasY(final Acceleration biasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasY = convertAcceleration(biasY);
    }

    /**
     * Gets known z coordinate of accelerometer bias.
     *
     * @return z coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasZAsAcceleration() {
        return new Acceleration(mBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z coordinate of accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z coordinate of accelerometer bias.
     *
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasZ(final Acceleration biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Sets known accelerometer bias coordinates expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasX x coordinate of accelerometer bias.
     * @param biasY y coordinate of accelerometer bias.
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasCoordinates(
            final double biasX, final double biasY, final double biasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasX = biasX;
        mBiasY = biasY;
        mBiasZ = biasZ;
    }

    /**
     * Sets known accelerometer bias coordinates.
     *
     * @param biasX z coordinate of accelerometer bias.
     * @param biasY y coordinate of accelerometer bias.
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if estimator is curently running.
     */
    @Override
    public void setBiasCoordinates(final Acceleration biasX, final Acceleration biasY,
                                   final Acceleration biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasX = convertAcceleration(biasX);
        mBiasY = convertAcceleration(biasY);
        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containint coordinates of known bias.
     */
    @Override
    public double[] getBias() {
File Line
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator.java 1597
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator.java 1577
            final SequentialRobustMixedRadioSourceEstimatorListener<S, P> listener)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(
            final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPowerdBm() {
        return mInitialTransmittedPowerdBm;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     *
     * @param initialTransmittedPowerdBm initial transmitted power to start the
     *                                   estimation of radio source transmitted
     *                                   power.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialTransmittedPowerdBm(
            final Double initialTransmittedPowerdBm) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialTransmittedPowerdBm = initialTransmittedPowerdBm;
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPower() {
        return mInitialTransmittedPowerdBm != null ?
                Utils.dBmToPower(mInitialTransmittedPowerdBm) : null;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     *
     * @param initialTransmittedPower initial transmitted power to start the
     *                                estimation of radio source transmitted power.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setInitialTransmittedPower(
            final Double initialTransmittedPower) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (initialTransmittedPower != null) {
            if (initialTransmittedPower < 0.0) {
                throw new IllegalArgumentException();
            }
            mInitialTransmittedPowerdBm = Utils.powerTodBm(
                    initialTransmittedPower);
        } else {
            mInitialTransmittedPowerdBm = null;
        }
    }

    /**
     * Gets initial position to start the estimation of radio source position.
     * If not defined, centroid of provided fingerprints will be used.
     *
     * @return initial position to start the estimation of radio source position.
     */
    public P getInitialPosition() {
        return mInitialPosition;
    }

    /**
     * Sets initial position to start the estimation of radio source position.
     * If not defined, centroid of provided fingerprints will be used.
     *
     * @param initialPosition initial position to start the estimation of radio
     *                        source position.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialPosition(
            final P initialPosition) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialPosition = initialPosition;
    }

    /**
     * Gets initial exponent typically used on free space for path loss propagation
     * in terms of distance.
     * On different environments path loss exponent might have different value:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     * <p>
     * If path loss exponent estimation is enabled, estimation will start at this
     * value and will converge to the most appropriate value.
     * If path loss exponent estimation is disabled, this value will be assumed
     * to be exact and the estimated path loss exponent will be equal to this
     * value.
     *
     * @return initial path loss exponent.
     */
    public double getInitialPathLossExponent() {
        return mInitialPathLossExponent;
    }

    /**
     * Sets initial exponent typically used on free space for path loss propagation
     * in terms of distance.
     * On different environments path loss exponent might have different value:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     * <p>
     * If path loss exponent estimation is enabled, estimation will start at this
     * value and will converge to the most appropriate value.
     * If path loss exponent estimation is disabled, this value will be assumed
     * to be exact and the estimated path loss exponent will be equal to this
     * value.
     *
     * @param initialPathLossExponent initial path loss exponent.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialPathLossExponent(
            final double initialPathLossExponent) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialPathLossExponent = initialPathLossExponent;
    }

    /**
     * Indicates whether transmitted power estimation is enabled or not.
     *
     * @return true if transmitted power estimation is enabled, false otherwise.
     */
    public boolean isTransmittedPowerEstimationEnabled() {
        return mTransmittedPowerEstimationEnabled;
    }

    /**
     * Specifies whether transmitted power estimation is enabled or not.
     *
     * @param transmittedPowerEstimationEnabled true if transmitted power estimation is enabled,
     *                                          false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setTransmittedPowerEstimationEnabled(
            final boolean transmittedPowerEstimationEnabled) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mTransmittedPowerEstimationEnabled = transmittedPowerEstimationEnabled;
    }

    /**
     * Indicates whether path loss estimation is enabled or not.
     *
     * @return true if path loss estimation is enabled, false otherwise.
     */
    public boolean isPathLossEstimationEnabled() {
        return mPathLossEstimationEnabled;
    }

    /**
     * Specifies whether path loss estimation is enabled or not.
     *
     * @param pathLossEstimationEnabled true if path loss estimation is enabled,
     *                                  false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setPathLossEstimationEnabled(
            final boolean pathLossEstimationEnabled) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mPathLossEstimationEnabled = pathLossEstimationEnabled;
    }

    /**
     * Indicates whether position covariances of readings must be taken into account to increase
     * the amount of standard deviation of each ranging measure by the amount of position standard
     * deviation assuming that both measures are statistically independent.
     *
     * @return true to take into account reading position covariances, false otherwise.
     */
    public boolean getUseReadingPositionCovariance() {
        return mUseReadingPositionCovariances;
    }

    /**
     * Specifies whether position covariances of readings must be taken into account to increase
     * the amount of standard deviation of each ranging measure by the amount of position standard
     * deviation assuming that both measures are statistically independent.
     *
     * @param useReadingPositionCovariances true to take into account reading position covariances, false
     *                                      otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setUseReadingPositionCovariances(
            final boolean useReadingPositionCovariances) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mUseReadingPositionCovariances = useReadingPositionCovariances;
    }

    /**
     * Indicates whether an homogeneous ranging linear solver is used to estimate preliminary
     * positions.
     *
     * @return true if homogeneous ranging linear solver is used, false if an inhomogeneous ranging linear
     * one is used instead.
     */
    public boolean isHomogeneousRangingLinearSolverUsed() {
        return mUseHomogeneousRangingLinearSolver;
    }

    /**
     * Specifies whether an homogeneous ranging linear solver is used to estimate preliminary
     * positions.
     *
     * @param useHomogeneousRangingLinearSolver true if homogeneous ranging linear solver is used, false
     *                                          if an inhomogeneous ranging linear one is used instead.
     * @throws LockedException if estimator is locked.
     */
    public void setHomogeneousRangingLinearSolverUsed(
            final boolean useHomogeneousRangingLinearSolver) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        mUseHomogeneousRangingLinearSolver = useHomogeneousRangingLinearSolver;
    }


    /**
     * Gets covariance for estimated position and power.
     * Matrix contains information in the following order:
     * Top-left submatrix contains covariance of position,
     * then follows transmitted power variance, and finally
     * the last element contains pathloss exponent variance.
     * This is only available when result has been refined and covariance is kept.
     *
     * @return covariance for estimated position and power.
     */
    public Matrix getCovariance() {
        return mCovariance;
    }

    /**
     * Gets estimated position covariance.
     * Size of this matrix will depend on the number of dimensions
     * of estimated position (either 2 or 3).
     * This is only available when result has been refined and covariance is kept.
     *
     * @return estimated position covariance.
     */
    public Matrix getEstimatedPositionCovariance() {
        return mEstimatedPositionCovariance;
    }

    /**
     * Gets estimated position.
     *
     * @return estimated position.
     */
    public P getEstimatedPosition() {
        return mEstimatedPosition;
    }

    /**
     * Indicates whether readings are valid or not.
     * Readings are considered valid when there are enough readings.
     *
     * @param readings readings to be validated.
     * @return true if readings are valid, false otherwise.
     */
    public boolean areValidReadings(
            final List<? extends ReadingLocated<P>> readings) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1096
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 627
            mRunning = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws AlgebraException if there are numerical errors.
     */
    private void calibrateCommonAxis() throws AlgebraException {
        // The gyroscope model is:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Hence:
        // [Ωmeasx] = [bx] + ( [1   0   0] + [sx    mxy    mxz]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0   1   0]   [myx   sy     myz]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0   0   1]   [mzx   mzy    sz ]  [Ωtruez]   [g31   g32   g33][ftruez]

        // where myx = mzx = mzy = 0

        // Hence:
        // [Ωmeasx] = [bx] + ( [1   0   0] + [sx    mxy    mxz]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0   1   0]   [0     sy     myz]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0   0   1]   [0     0      sz ]  [Ωtruez]   [g31   g32   g33][ftruez]


        // [Ωmeasx] = [bx] + ( [1+sx  mxy    mxz ]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0     1+sy   myz ]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0     0      1+sz]  [Ωtruez]   [g31   g32   g33][ftruez]

        // Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myz, g11, g12, g13, g21, g22, g23, g31, g32, g33
        // Reordering:
        // Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Ωmeasx - Ωtruex - bx = sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy - Ωtruey - by = sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz - Ωtruez - bz = sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // [Ωtruex  0       0       Ωtruey  Ωtruez  0       ftruex  ftruey  ftruez  0       0       0       0       0       0     ][sx ] =  [Ωmeasx - Ωtruex - bx]
        // [0       Ωtruey  0       0       0       Ωtruez  0       0       0       ftruex  ftruey  ftruez  0       0       0     ][sy ]    [Ωmeasy - Ωtruey - by]
        // [0       0       Ωtruez  0       0       0       0       0       0       0       0       0       ftruex  ftruey  ftruez][sz ]    [Ωmeasz - Ωtruez - bz]
        //                                                                                                                         [mxy]
        //                                                                                                                         [mxz]
        //                                                                                                                         [myz]
        //                                                                                                                         [g11]
        //                                                                                                                         [g12]
        //                                                                                                                         [g13]
        //                                                                                                                         [g21]
        //                                                                                                                         [g22]
        //                                                                                                                         [g23]
        //                                                                                                                         [g31]
        //                                                                                                                         [g32]
        //                                                                                                                         [g33]

        final BodyKinematics expectedKinematics = new BodyKinematics();

        final int rows = EQUATIONS_PER_MEASUREMENT * mMeasurements.size();
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double omegaMeasX = measuredKinematics.getAngularRateX();
            final double omegaMeasY = measuredKinematics.getAngularRateY();
            final double omegaMeasZ = measuredKinematics.getAngularRateZ();

            final double omegaTrueX = expectedKinematics.getAngularRateX();
            final double omegaTrueY = expectedKinematics.getAngularRateY();
            final double omegaTrueZ = expectedKinematics.getAngularRateZ();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, omegaTrueX);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6317
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1761
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS ||
                initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMa.getElementAtIndex(0);
        mInitialMyx = initialMa.getElementAtIndex(1);
        mInitialMzx = initialMa.getElementAtIndex(2);

        mInitialMxy = initialMa.getElementAtIndex(3);
        mInitialSy = initialMa.getElementAtIndex(4);
        mInitialMzy = initialMa.getElementAtIndex(5);

        mInitialMxz = initialMa.getElementAtIndex(6);
        mInitialMyz = initialMa.getElementAtIndex(7);
        mInitialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return mPosition;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final NEDPosition result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (mPosition != null) {
            final NEDVelocity velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    mPosition.getX(), mPosition.getY(), mPosition.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6757
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1719
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS ||
                initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMa.getElementAtIndex(0);
        mInitialMyx = initialMa.getElementAtIndex(1);
        mInitialMzx = initialMa.getElementAtIndex(2);

        mInitialMxy = initialMa.getElementAtIndex(3);
        mInitialSy = initialMa.getElementAtIndex(4);
        mInitialMzy = initialMa.getElementAtIndex(5);

        mInitialMxz = initialMa.getElementAtIndex(6);
        mInitialMyz = initialMa.getElementAtIndex(7);
        mInitialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return mPosition;
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final NEDPosition result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (mPosition != null) {
            final NEDVelocity velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    mPosition.getX(), mPosition.getY(), mPosition.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3274
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3804
        if (mEstimatedMa == null) {
            mEstimatedMa = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
        } else {
            mEstimatedMa.initialize(0.0);
        }

        mEstimatedMa.setElementAt(0, 0, sx);
        mEstimatedMa.setElementAt(1, 0, myx);
        mEstimatedMa.setElementAt(2, 0, mzx);

        mEstimatedMa.setElementAt(0, 1, mxy);
        mEstimatedMa.setElementAt(1, 1, sy);
        mEstimatedMa.setElementAt(2, 1, mzy);

        mEstimatedMa.setElementAt(0, 2, mxz);
        mEstimatedMa.setElementAt(1, 2, myz);
        mEstimatedMa.setElementAt(2, 2, sz);

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter.
     *
     * @throws WrongSizeException never happens.
     */
    private void setInputData() throws WrongSizeException {
        // set input data using:
        //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
        //  fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

        final BodyKinematics expectedKinematics = new BodyKinematics();

        final int numMeasurements = mMeasurements.size();
        final Matrix x = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
        final Matrix y = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
        final double[] specificForceStandardDeviations = new double[numMeasurements];
        int i = 0;
        for (final StandardDeviationFrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double fMeasX = measuredKinematics.getFx();
            final double fMeasY = measuredKinematics.getFy();
            final double fMeasZ = measuredKinematics.getFz();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            x.setElementAt(i, 0, fTrueX);
            x.setElementAt(i, 1, fTrueY);
            x.setElementAt(i, 2, fTrueZ);

            y.setElementAt(i, 0, fMeasX);
            y.setElementAt(i, 1, fMeasY);
            y.setElementAt(i, 2, fMeasZ);

            specificForceStandardDeviations[i] =
                    measurement.getSpecificForceStandardDeviation();
            i++;
        }

        mFitter.setInputData(x, y, specificForceStandardDeviations);
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(),
                acceleration.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }
}
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 861
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 996
    public INSLooselyCoupledKalmanState(final INSLooselyCoupledKalmanState input) {
        copyFrom(input);
    }

    /**
     * Gets estimated body to ECEF coordinate transformation matrix.
     *
     * @return estimated body to ECEF coordinate transformation matrix.
     */
    public Matrix getBodyToEcefCoordinateTransformationMatrix() {
        return mBodyToEcefCoordinateTransformationMatrix;
    }

    /**
     * Sets estimated body to ECEF coordinate transformation matrix.
     *
     * @param bodyToEcefCoordinateTransformationMatrix estimated body to ECEF coordinate
     *                                                 transformation matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setBodyToEcefCoordinateTransformationMatrix(
            final Matrix bodyToEcefCoordinateTransformationMatrix) {
        if (bodyToEcefCoordinateTransformationMatrix.getRows() != CoordinateTransformation.ROWS ||
                bodyToEcefCoordinateTransformationMatrix.getColumns() != CoordinateTransformation.COLS) {
            throw new IllegalArgumentException();
        }
        mBodyToEcefCoordinateTransformationMatrix =
                bodyToEcefCoordinateTransformationMatrix;
    }

    /**
     * Gets estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
     *
     * @return estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
     */
    public double getVx() {
        return mVx;
    }

    /**
     * Sets estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
     *
     * @param vx estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
     */
    public void setVx(final double vx) {
        mVx = vx;
    }

    /**
     * Gets estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
     *
     * @return estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
     */
    public double getVy() {
        return mVy;
    }

    /**
     * Sets estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
     *
     * @param vy estimated ECEF user velocity resolved around y axis and expressed in meters per scond (m/s).
     */
    public void setVy(final double vy) {
        mVy = vy;
    }

    /**
     * Gets estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
     *
     * @return estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
     */
    public double getVz() {
        return mVz;
    }

    /**
     * Sets estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
     *
     * @param vz estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
     */
    public void setVz(final double vz) {
        mVz = vz;
    }

    /**
     * Sets estimated ECEF user velocity coordinates.
     *
     * @param vx estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
     * @param vy estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
     * @param vz estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
     */
    public void setVelocityCoordinates(
            final double vx, final double vy, final double vz) {
        mVx = vx;
        mVy = vy;
        mVz = vz;
    }

    /**
     * Gets x coordinate of estimated ECEF user position expressed in meters (m).
     *
     * @return x coordinate of estimated ECEF user position expressed in meters (m).
     */
    public double getX() {
        return mX;
    }

    /**
     * Sets x coordinate of estimated ECEF user position expressed in meters (m).
     *
     * @param x x coordinate of estimated ECEF user position expressed in meters (m).
     */
    public void setX(final double x) {
        mX = x;
    }

    /**
     * Gets y coordinate of estimated ECEF user position expressed in meters (m).
     *
     * @return y coordinate of estimated ECEF user position expressed in meters (m).
     */
    public double getY() {
        return mY;
    }

    /**
     * Sets y coordinate of estimated ECEF user position expressed in meters (m).
     *
     * @param y y coordinate of estimated ECEF user position expressed in meters (m).
     */
    public void setY(final double y) {
        mY = y;
    }

    /**
     * Gets z coordinate of estimated ECEF user position expressed in meters (m).
     *
     * @return z coordinate of estimated ECEF user position expressed in meters (m).
     */
    public double getZ() {
        return mZ;
    }

    /**
     * Sets z coordinate of estimated ECEF user position expressed in meters (m).
     *
     * @param z z coordinate of estimated ECEF user position expressed in meters (m).
     */
    public void setZ(final double z) {
        mZ = z;
    }

    /**
     * Sets estimated ECEF user position coordinates.
     *
     * @param x x coordinate of estimated ECEF user position expressed in meters (m).
     * @param y y coordinate of estimated ECEF user position expressed in meters (m).
     * @param z z coordinate of estimated ECEF user position expressed in meters (m).
     */
    public void setPositionCoordinates(
            final double x, final double y, final double z) {
        mX = x;
        mY = y;
        mZ = z;
    }

    /**
     * Gets estimated accelerometer bias resolved around x axis and expressed in
     * meters per squared second (m/s^2).
     *
     * @return estimated accelerometer bias resolved around x axis and expressed in
     * meters per squared second (m/s^2).
     */
    public double getAccelerationBiasX() {
        return mAccelerationBiasX;
    }

    /**
     * Sets estimated accelerometer bias resolved around x axis and expressed in
     * meters per squared second (m/s^2).
     *
     * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
     *                          expressed in meters per squared second (m/s^2).
     */
    public void setAccelerationBiasX(final double accelerationBiasX) {
        mAccelerationBiasX = accelerationBiasX;
    }

    /**
     * Gets estimated accelerometer bias resolved around y axis and expressed in
     * meters per squared second (m/s^2).
     *
     * @return estimated accelerometer bias resolved around y axis and expressed in
     * meters per squared second (m/s^2).
     */
    public double getAccelerationBiasY() {
        return mAccelerationBiasY;
    }

    /**
     * Sets estimated accelerometer bias resolved around y axis and expressed in
     * meters per squared second (m/s^2).
     *
     * @param accelerationBiasY estimated accelerometer bias resolved around y axis
     *                          and expressed in meters per squared second (m/s^2).
     */
    public void setAccelerationBiasY(final double accelerationBiasY) {
        mAccelerationBiasY = accelerationBiasY;
    }

    /**
     * Gets estimated accelerometer bias resolved around z axis and expressed in
     * meters per squared second (m/s^2).
     *
     * @return estimated accelerometer bias resolved around z axis and
     * expressed in meters per squared second (m/s^2).
     */
    public double getAccelerationBiasZ() {
        return mAccelerationBiasZ;
    }

    /**
     * Sets estimated accelerometer bias resolved around z axis and expressed in
     * meters per squared second (m/s^2).
     *
     * @param accelerationBiasZ estimated accelerometer bias resolved around z axis
     *                          and expressed in meters per squared second (m/s^2).
     */
    public void setAccelerationBiasZ(final double accelerationBiasZ) {
        mAccelerationBiasZ = accelerationBiasZ;
    }

    /**
     * Sets estimated accelerometer bias expressed in meters per squared second (m/s^2).
     *
     * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
     *                          expressed in meters per squared second (m/s^2).
     * @param accelerationBiasY estimated accelerometer bias resolved around y axis and
     *                          expressed in meters per squared second (m/s^2).
     * @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
     *                          expressed in meters per squared second (m/s^2).
     */
    public void setAccelerationBiasCoordinates(
            final double accelerationBiasX, final double accelerationBiasY,
            final double accelerationBiasZ) {
        mAccelerationBiasX = accelerationBiasX;
        mAccelerationBiasY = accelerationBiasY;
        mAccelerationBiasZ = accelerationBiasZ;
    }

    /**
     * Gets estimated gyroscope bias resolved around x axis and expressed in
     * radians per second (rad/s).
     *
     * @return estimated gyroscope bias resolved around x axis and expressed in
     * radians per second (rad/s).
     */
    public double getGyroBiasX() {
        return mGyroBiasX;
    }

    /**
     * Sets estimated gyroscope bias resolved around x axis and expressed in
     * radians per second (rad/s).
     *
     * @param gyroBiasX estimated gyroscope bias resolved around x axis and
     *                  expressed in radians per second (rad/s).
     */
    public void setGyroBiasX(final double gyroBiasX) {
        mGyroBiasX = gyroBiasX;
    }

    /**
     * Gets estimated gyroscope bias resolved around y axis and expressed in
     * radians per second (rad/s).
     *
     * @return estimated gyroscope bias resolved around y axis and expressed
     * in radians per second (rad/s).
     */
    public double getGyroBiasY() {
        return mGyroBiasY;
    }

    /**
     * Sets estimated gyroscope bias resolved around y axis and expressed in
     * radians per second (rad/s).
     *
     * @param gyroBiasY estimated gyroscope bias resolved around y axis and
     *                  expressed in radians per second (rad/s).
     */
    public void setGyroBiasY(final double gyroBiasY) {
        mGyroBiasY = gyroBiasY;
    }

    /**
     * Gets estimated gyroscope bias resolved around z axis and expressed in
     * radians per second (rad/s).
     *
     * @return estimated gyroscope bias resolved around z axis and expressed
     * in radians per second (rad/s).
     */
    public double getGyroBiasZ() {
        return mGyroBiasZ;
    }

    /**
     * Sets estimated gyroscope bias resolved around z axis and expressed in
     * radians per second (rad/s).
     *
     * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
     *                  expressed in radians per second (rad/s).
     */
    public void setGyroBiasZ(final double gyroBiasZ) {
        mGyroBiasZ = gyroBiasZ;
    }

    /**
     * Sets estimated gyroscope bias coordinates expressed in radians
     * per second (rad/s).
     *
     * @param gyroBiasX estimated gyroscope bias resolved around x axis and
     *                  expressed in radians per second (rad/s).
     * @param gyroBiasY estimated gyroscope bias resolved around y axis and
     *                  expressed in radians per second (rad/s).
     * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
     *                  expressed in radians per second (rad/s).
     */
    public void setGyroBiasCoordinates(
            final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ) {
        mGyroBiasX = gyroBiasX;
        mGyroBiasY = gyroBiasY;
        mGyroBiasZ = gyroBiasZ;
    }

    /**
     * Gets Kalman filter error covariance matrix.
     * Notice that covariance is expressed in terms of ECEF coordinates.
     * If accuracy of position, attitude or velocity needs to be expressed in terms
     * of NED coordinates, their respective submatrices of this covariance matrix
     * must be rotated, taking into account the Jacobian of the matrix transformation
     * relating both coordinates, the covariance can be expressed following the law
     * of propagation of uncertainties (https://en.wikipedia.org/wiki/Propagation_of_uncertainty)
     * as: cov(f(x)) = J*cov(x)*J'.
     *
     * @param result instance where result data will be copied to.
     * @return true if result data has been copied, false otherwise.
     */
    public boolean getCovariance(final Matrix result) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4282
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3976
            mB.setElementAtIndex(2, bz);

            mG.setElementAt(0, 0, g11);
            mG.setElementAt(1, 0, g21);
            mG.setElementAt(2, 0, g31);

            mG.setElementAt(0, 1, g12);
            mG.setElementAt(1, 1, g22);
            mG.setElementAt(2, 1, g32);

            mG.setElementAt(0, 2, g13);
            mG.setElementAt(1, 2, g23);
            mG.setElementAt(2, 2, g33);

            // fix kinematics
            final int numItems = measuredSequence.getItemsCount();
            final List<StandardDeviationTimedBodyKinematics> measuredItems = measuredSequence.getSortedItems();
            final List<StandardDeviationTimedBodyKinematics> fixedItems = fixedSequence.getSortedItems();
            for (int j = 0; j < numItems; j++) {
                final StandardDeviationTimedBodyKinematics measuredItem = measuredItems.get(j);
                final StandardDeviationTimedBodyKinematics fixedItem = fixedItems.get(j);

                fixKinematics(measuredItem.getKinematics(), fixedItem.getKinematics());
            }


            // integrate fixed sequence to obtain attitude change
            QuaternionIntegrator.integrateGyroSequence(fixedSequence, mQ);

            mStartPoint.setInhomogeneousCoordinates(
                    mPoint[0], mPoint[1], mPoint[2]);
            mQ.inverse();
            mQ.rotate(mStartPoint, mEndPoint);

            mExpectedEndPoint.setInhomogeneousCoordinates(
                    mPoint[3], mPoint[4], mPoint[5]);

            return mExpectedEndPoint.distanceTo(mEndPoint);

        } catch (final AlgebraException e) {
            throw new EvaluationException(e);
        }
    }

    /**
     * Fixes provided kinematics with provided accelerometer paramenters and
     * current gyroscope parameters.
     *
     * @param kinematics kinematics to be fixed with current values.
     * @param result     kinematics where result will be stored.
     * @throws AlgebraException if for some reason kinematics
     */
    private void fixKinematics(
            final BodyKinematics kinematics,
            final BodyKinematics result) throws AlgebraException {

        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
        // Ωmeas = M*(Ωtrue + b + G * ftrue)

        // M = I + Mg
        // bg = M*b --> b = M^-1*bg
        // Gg = M*G --> G = M^-1*Gg

        // Ωtrue = M^-1 * Ωmeas - b - G*ftrue

        // fix specific force
        mMeasuredSpecificForce.setElementAtIndex(0, kinematics.getFx());
        mMeasuredSpecificForce.setElementAtIndex(1, kinematics.getFy());
        mMeasuredSpecificForce.setElementAtIndex(2, kinematics.getFz());

        mAccelerationFixer.fix(mMeasuredSpecificForce, mTrueSpecificForce);

        // fix angular rate
        mMeasuredAngularRate.setElementAtIndex(0, kinematics.getAngularRateX());
        mMeasuredAngularRate.setElementAtIndex(1, kinematics.getAngularRateY());
        mMeasuredAngularRate.setElementAtIndex(2, kinematics.getAngularRateZ());

        mG.multiply(mTrueSpecificForce, mTmp);
        mInvM.multiply(mMeasuredAngularRate, mTrueAngularRate);
        mTrueAngularRate.subtract(mB);
        mTrueAngularRate.subtract(mTmp);

        result.setSpecificForceCoordinates(
                mTrueSpecificForce.getElementAtIndex(0),
                mTrueSpecificForce.getElementAtIndex(1),
                mTrueSpecificForce.getElementAtIndex(2));

        result.setAngularRateCoordinates(
                mTrueAngularRate.getElementAtIndex(0),
                mTrueAngularRate.getElementAtIndex(1),
                mTrueAngularRate.getElementAtIndex(2));
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3472
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4993
        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];
        final double m21 = result[4];
        final double m31 = result[5];

        final double m12 = result[6];
        final double m22 = result[7];
        final double m32 = result[8];

        final double m13 = result[9];
        final double m23 = result[10];
        final double m33 = result[11];

        final double g11 = result[12];
        final double g21 = result[13];
        final double g31 = result[14];

        final double g12 = result[15];
        final double g22 = result[16];
        final double g32 = result[17];

        final double g13 = result[18];
        final double g23 = result[19];
        final double g33 = result[20];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
        b.setElementAtIndex(0, bx);
        b.setElementAtIndex(1, by);
        b.setElementAtIndex(2, bz);

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, m21);
        m.setElementAtIndex(2, m31);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, m32);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        final Matrix g = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        g.setElementAtIndex(0, g11);
        g.setElementAtIndex(1, g21);
        g.setElementAtIndex(2, g31);

        g.setElementAtIndex(3, g12);
        g.setElementAtIndex(4, g22);
        g.setElementAtIndex(5, g32);

        g.setElementAtIndex(6, g13);
        g.setElementAtIndex(7, g23);
        g.setElementAtIndex(8, g33);

        setResult(m, b, g);
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and G-dependent cross biases
     * are ignored.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxis()
            throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 602
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3739
        } catch (final AlgebraException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    @Override
    public double[] getEstimatedHardIron() {
        return mEstimatedHardIron;
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where estimated magnetometer biases will be
     *               stored.
     * @return true if result instance was updated, false otherwise (when
     * estimation is not yet available).
     */
    @Override
    public boolean getEstimatedHardIron(final double[] result) {
        if (mEstimatedHardIron != null) {
            System.arraycopy(mEstimatedHardIron, 0, result,
                    0, mEstimatedHardIron.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @return column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases.
     */
    @Override
    public Matrix getEstimatedHardIronAsMatrix() {
        return mEstimatedHardIron != null ? Matrix.newFromArray(mEstimatedHardIron) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedHardIronAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedHardIron != null) {
            result.fromArray(mEstimatedHardIron);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return x coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronX() {
        return mEstimatedHardIron != null ? mEstimatedHardIron[0] : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return y coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronY() {
        return mEstimatedHardIron != null ? mEstimatedHardIron[1] : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return z coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronZ() {
        return mEstimatedHardIron != null ? mEstimatedHardIron[2] : null;
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 842
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 855
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Solution<Point3D>> innerEstimator =
                new PROSACRobustEstimator<>(
                        new PROSACRobustEstimatorListener<Solution<Point3D>>() {

                            @Override
                            public double[] getQualityScores() {
                                return mQualityScores;
                            }

                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point3D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point3D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return PROSACRobustRangingAndRssiRadioSourceEstimator3D.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1677
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 936
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1748
    public void getHardIronAsMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mHardIronX);
        result.setElementAtIndex(1, mHardIronY);
        result.setElementAtIndex(2, mHardIronZ);
    }

    /**
     * Sets known hard-iron bias as a column matrix.
     *
     * @param hardIron known hard-iron bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setHardIron(final Matrix hardIron)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS
                || hardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mHardIronX = hardIron.getElementAtIndex(0);
        mHardIronY = hardIron.getElementAtIndex(1);
        mHardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMm(final Matrix initialMm)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS ||
                initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMm.getElementAtIndex(0);
        mInitialMyx = initialMm.getElementAtIndex(1);
        mInitialMzx = initialMm.getElementAtIndex(2);

        mInitialMxy = initialMm.getElementAtIndex(3);
        mInitialSy = initialMm.getElementAtIndex(4);
        mInitialMzy = initialMm.getElementAtIndex(5);

        mInitialMxz = initialMm.getElementAtIndex(6);
        mInitialMyz = initialMm.getElementAtIndex(7);
        mInitialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken.
     *
     * @return position where body magnetic flux density measurements have
     * been taken.
     */
    public NEDPosition getNedPosition() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1158
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1133
            final RobustKnownFrameMagnetometerCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Indicates whether calibrator is ready to start the estimator.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or no.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Gets Earth's magnetic model.
     *
     * @return Earth's magnetic model or null if not provided.
     */
    public WorldMagneticModel getMagneticModel() {
        return mMagneticModel;
    }

    /**
     * Sets Earth's magnetic model.
     *
     * @param magneticModel Earth's magnetic model to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMagneticModel(final WorldMagneticModel magneticModel)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMagneticModel = magneticModel;
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return mUseLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mUseLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return mRefinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mRefinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    public double[] getEstimatedHardIron() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2469
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2475
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1103
        mBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1089
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 612
            mRunning = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws AlgebraException if there are numerical errors.
     */
    private void calibrateCommonAxis() throws AlgebraException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // Hence:
        //  [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        //  [fmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [ftruey]
        //  [fmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [ftruez]

        // where myx = mzx = mzy = 0

        // Hence:
        //  [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        //  [fmeasy] = [by]     [0  1   0]   [0     sy  myz]    [ftruey]
        //  [fmeasz] = [bz]     [0  0   1]   [0     0   sz ]    [ftruez]

        //  [fmeasx] = [bx] +   [1+sx   mxy     mxz ][ftruex]
        //  [fmeasy]   [by]     [0      1+sy    myz ][ftruey]
        //  [fmeasz]   [bz]     [0      0       1+sz][ftruez]

        //  fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + (1+sy) * ftruey + myz * ftruez
        //  fmeasz = bz + (1+sz) * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myz
        // Reordering:
        //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + ftruey + sy * ftruey + myz * ftruez
        //  fmeasz = bz + ftruez + sz * ftruez

        //  fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy - ftruey - by = sy * ftruey + myz * ftruez
        //  fmeasz - ftruez - bz = sz * ftruez

        // [ftruex  0       0       ftruey  ftruez  0     ][sx ] = [fmeasx - ftruex - bx]
        // [0       ftruey  0       0       0       ftruez][sy ]   [fmeasy - ftruey - by]
        // [0       0       ftruez  0       0       0     ][sz ]   [fmeasz - ftruez - bz]
        //                                                 [mxy]
        //                                                 [mxz]
        //                                                 [myz]

        final BodyKinematics expectedKinematics = new BodyKinematics();

        final int rows = EQUATIONS_PER_MEASUREMENT * mMeasurements.size();
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double fMeasX = measuredKinematics.getFx();
            final double fMeasY = measuredKinematics.getFy();
            final double fMeasZ = measuredKinematics.getFz();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, fTrueX);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2469
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2475
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3719
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3764
        mBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2511
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2469
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3701
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2513
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2475
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3659
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1103
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2483
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1381
        estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity,
                position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final double timeInterval,
                                          final CoordinateTransformation c,
                                          final CoordinateTransformation oldC,
                                          final Speed vx, final Speed vy, final Speed vz,
                                          final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                          final Point3D position, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final Time timeInterval,
                                          final CoordinateTransformation c,
                                          final CoordinateTransformation oldC,
                                          final Speed vx, final Speed vy, final Speed vz,
                                          final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                          final Point3D position, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static BodyKinematics estimateKinematicsAndReturnNew(final double timeInterval,
                                                                final CoordinateTransformation c,
                                                                final CoordinateTransformation oldC,
                                                                final double vx, final double vy, final double vz,
                                                                final double oldVx, final double oldVy, final double oldVz,
                                                                final double x, final double y, final double z) {
        final BodyKinematics result = new BodyKinematics();
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                x, y, z, result);
        return result;
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static BodyKinematics estimateKinematicsAndReturnNew(final Time timeInterval,
                                                                final CoordinateTransformation c,
                                                                final CoordinateTransformation oldC,
                                                                final double vx, final double vy, final double vz,
                                                                final double oldVx, final double oldVy, final double oldVz,
                                                                final double x, final double y, final double z) {
        final BodyKinematics result = new BodyKinematics();
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                x, y, z, result);
        return result;
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static BodyKinematics estimateKinematicsAndReturnNew(final double timeInterval,
                                                                final CoordinateTransformation c,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3304
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4792
        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];

        final double m12 = result[4];
        final double m22 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final double g11 = result[9];
        final double g21 = result[10];
        final double g31 = result[11];

        final double g12 = result[12];
        final double g22 = result[13];
        final double g32 = result[14];

        final double g13 = result[15];
        final double g23 = result[16];
        final double g33 = result[17];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
        b.setElementAtIndex(0, bx);
        b.setElementAtIndex(1, by);
        b.setElementAtIndex(2, bz);

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, 0.0);
        m.setElementAtIndex(2, 0.0);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        final Matrix g = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        g.setElementAtIndex(0, g11);
        g.setElementAtIndex(1, g21);
        g.setElementAtIndex(2, g31);

        g.setElementAtIndex(3, g12);
        g.setElementAtIndex(4, g22);
        g.setElementAtIndex(5, g32);

        g.setElementAtIndex(6, g13);
        g.setElementAtIndex(7, g23);
        g.setElementAtIndex(8, g33);

        setResult(m, b, g);
    }

    /**
     * Internal method to perform general calibration when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneralAndGDependentCrossBiases()
            throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2511
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2469
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2513
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1638
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2475
        mInitialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 840
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 331
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Solution<Point2D>> innerEstimator =
                new PROSACRobustEstimator<>(
                        new PROSACRobustEstimatorListener<Solution<Point2D>>() {

                            @Override
                            public double[] getQualityScores() {
                                return mQualityScores;
                            }

                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point2D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point2D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return PROSACRobustRangingAndRssiRadioSourceEstimator2D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 844
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 330
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 857
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Solution<Point3D>> innerEstimator =
                new PROSACRobustEstimator<>(
                        new PROSACRobustEstimatorListener<Solution<Point3D>>() {

                            @Override
                            public double[] getQualityScores() {
                                return mQualityScores;
                            }

                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point3D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point3D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return PROSACRobustRangingAndRssiRadioSourceEstimator3D.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3701
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1638
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3659
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3719
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3764
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    public Matrix getInitialGg() {
        return new Matrix(mInitialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(mInitialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS
                || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(mInitialGg);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @return constant rotation rate of turntable.
     */
    public double getTurntableRotationRate() {
File Line
com/irurueta/navigation/indoor/Utils.java 260
com/irurueta/navigation/indoor/Utils.java 799
    public static MultivariateNormalDist propagateVariancesToRssiVarianceFirstOrderNonLinear2D(
            final double fingerprintRssi, final double pathLossExponent,
            final Point2D fingerprintPosition, final Point2D radioSourcePosition,
            final Point2D estimatedPosition,
            final Double fingerprintRssiVariance,
            final Double pathLossExponentVariance,
            final Matrix fingerprintPositionCovariance,
            final Matrix radioSourcePositionCovariance,
            final Matrix estimatedPositionCovariance) throws IndoorException {

        if (fingerprintPosition == null || radioSourcePosition == null ||
                estimatedPosition == null) {
            return null;
        }

        //1st order Taylor expression of received power in 2D:
        //Pr(pi) = Pr(p1)
        //  - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
        //  - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
        //where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2

        final double x1 = fingerprintPosition.getInhomX();
        final double y1 = fingerprintPosition.getInhomY();

        final double xa = radioSourcePosition.getInhomX();
        final double ya = radioSourcePosition.getInhomY();

        final double xi = estimatedPosition.getInhomX();
        final double yi = estimatedPosition.getInhomY();

        final double[] mean = new double[]{
                fingerprintRssi, pathLossExponent, x1, y1, xa, ya, xi, yi
        };
        final Matrix covariance = Matrix.diagonal(new double[]{
                fingerprintRssiVariance != null ? fingerprintRssiVariance : 0.0,
                pathLossExponentVariance != null ? pathLossExponentVariance : 0.0,
                0.0, 0.0, 0.0, 0.0, 0.0, 0.0
        });

        if (fingerprintPositionCovariance != null &&
                fingerprintPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH &&
                fingerprintPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {

            covariance.setSubmatrix(2, 2,
                    3, 3,
                    fingerprintPositionCovariance);
        }

        if (radioSourcePositionCovariance != null &&
                radioSourcePositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH &&
                radioSourcePositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
            covariance.setSubmatrix(4, 4,
                    5, 5,
                    radioSourcePositionCovariance);
        }

        if (estimatedPositionCovariance != null &&
                estimatedPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH &&
                estimatedPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
            covariance.setSubmatrix(6, 6,
                    7, 7,
                    estimatedPositionCovariance);
        }

        try {
            return MultivariateNormalDist.propagate(new MultivariateNormalDist.JacobianEvaluator() {
                @Override
                public void evaluate(
                        final double[] x, final double[] y, final Matrix jacobian) {

                    //Pr(pi) = Pr(p1)
                    //  - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
                    //  - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
                    //where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2

                    //Hence:
                    //Pr(pi) = Pr(p1) -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/
                    //      (ln(10)*((x1 - xa)^2 + (y1 - ya)^2))

                    final double diffX1a = x1 - xa;
                    final double diffY1a = y1 - ya;

                    final double diffXi1 = xi - x1;
                    final double diffYi1 = yi - y1;

                    final double diffX1a2 = diffX1a * diffX1a;
                    final double diffY1a2 = diffY1a * diffY1a;

                    final double d1a2 = diffX1a2 + diffY1a2;
File Line
com/irurueta/navigation/indoor/fingerprint/BaseFingerprintPositionAndRadioSourceEstimator.java 163
com/irurueta/navigation/indoor/fingerprint/BaseFingerprintPositionEstimator.java 170
    public BaseFingerprintPositionAndRadioSourceEstimator(
            final List<? extends RssiFingerprintLocated<? extends RadioSource,
                    ? extends RssiReading<? extends RadioSource>, P>> locatedFingerprints,
            final RssiFingerprint<? extends RadioSource,
                    ? extends RssiReading<? extends RadioSource>> fingerprint,
            final L listener) {
        this(listener);
        internalSetLocatedFingerprints(locatedFingerprints);
        internalSetFingerprint(fingerprint);
    }

    /**
     * Gets located fingerprints containing RSSI readings.
     *
     * @return located fingerprints containing RSSI readings.
     */
    public List<? extends RssiFingerprintLocated<? extends RadioSource,
            ? extends RssiReading<? extends RadioSource>, P>> getLocatedFingerprints() {
        return mLocatedFingerprints;
    }

    /**
     * Sets located fingerprints containing RSSI readings.
     *
     * @param locatedFingerprints located fingerprints containing RSSI readings.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is null or there are not enough
     *                                  fingerprints or readings within provided fingerprints (for 2D position estimation at
     *                                  least 2 readings are required in a single fingerprint, or at least 2 fingerprints
     *                                  at different locations containing a single reading are required. For 3D position
     *                                  estimation 3 reading in a single fingerprint, or 3 fingerprints containing a single
     *                                  reading or any combination resulting in at least 3 readings at different locations
     *                                  are required).
     */
    public void setLocatedFingerprints(
            final List<? extends RssiFingerprintLocated<? extends RadioSource,
                    ? extends RssiReading<? extends RadioSource>, P>> locatedFingerprints)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        internalSetLocatedFingerprints(locatedFingerprints);
    }

    /**
     * Gets fingerprint containing readings at an unknown location for provided located
     * fingerprints.
     *
     * @return fingerprint containing readings at an unknown location for provided located
     * fingerprints.
     */
    public RssiFingerprint<? extends RadioSource, ? extends RssiReading<? extends RadioSource>>
    getFingerprint() {
        return mFingerprint;
    }

    /**
     * Sets fingerprint containing readings at an unknown location for provided located fingerprints.
     *
     * @param fingerprint fingerprint containing readings at an unknown location for provided located fingerprints.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is null.
     */
    public void setFingerprint(
            final RssiFingerprint<? extends RadioSource,
                    ? extends RssiReading<? extends RadioSource>> fingerprint)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        internalSetFingerprint(fingerprint);
    }

    /**
     * Get minimum number of nearest fingerprints to search.
     *
     * @return minimum number of nearest fingerprints, -1 indicates to initially use
     * all fingerprints needed to estimate available radio sources.
     */
    public int getMinNearestFingerprints() {
        return mMinNearestFingerprints;
    }

    /**
     * Gets maximum number of nearest fingerprints to search.
     *
     * @return maximum number of nearest fingerprints, -1 indicates to use all available
     * fingerprints.
     */
    public int getMaxNearestFingerprints() {
        return mMaxNearestFingerprints;
    }

    /**
     * Sets minimum and maximum number of nearest fingerprints to search.
     * If minimum value is -1, then the minimum required number of fingerprints needed
     * to estimate available radio sources is used.
     * If maximum value is -1, then the problem is attempted to be solved until all
     * available fingerprints are used.
     *
     * @param minNearestFingerprints minimum number of nearest fingerprints or -1.
     * @param maxNearestFingerprints maximum number of nearest fingerprints or -1.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if minimum value is larger than maximum value (as
     *                                  long as it has a limit defined), or if maximum value is not negative when
     *                                  minimum one is less than 1, or if minimum value is zero.
     */
    public void setMinMaxNearestFingerprints(
            final int minNearestFingerprints,
            final int maxNearestFingerprints) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        internalSetMinMaxNearestFingerprints(minNearestFingerprints,
                maxNearestFingerprints);
    }

    /**
     * Gets path loss exponent to be used by default.
     * This is typically used on free space for path loss propagation in
     * terms of distance.
     * On different environments path loss exponent might have different values:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     *
     * @return path loss exponent to be used by default.
     */
    public double getPathLossExponent() {
        return mPathLossExponent;
    }

    /**
     * Sets path loss exponent to be used by default.
     * This is typically used on free space for path loss propagation in
     * terms of distance.
     * On different environments path loss exponent might have different values:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     *
     * @param pathLossExponent path loss exponent to be used by default.
     * @throws LockedException if estimator is locked.
     */
    public void setPathLossExponent(final double pathLossExponent) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mPathLossExponent = pathLossExponent;
    }

    /**
     * Gets listener to be notified of events raised by this instance.
     *
     * @return listener to be notified of events raised by this instance.
     */
    public L getListener() {
        return mListener;
    }

    /**
     * Sets listener to be notified of events raised by this instance.
     *
     * @param listener listener to be notified of events raised by this instance.
     * @throws LockedException if estimator is locked.
     */
    public void setListener(final L listener) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mListener = listener;
    }

    /**
     * Gets estimated inhomogeneous position coordinates.
     *
     * @return estimated inhomogeneous position coordinates.
     */
    public double[] getEstimatedPositionCoordinates() {
        return mEstimatedPositionCoordinates;
    }

    /**
     * Gets estimated estimated position and stores result into provided instance.
     *
     * @param estimatedPosition instance where estimated estimated position will be stored.
     */
    public void getEstimatedPosition(final P estimatedPosition) {
        if (mEstimatedPositionCoordinates != null) {
            for (int i = 0; i < mEstimatedPositionCoordinates.length; i++) {
                estimatedPosition.setInhomogeneousCoordinate(i,
                        mEstimatedPositionCoordinates[i]);
            }
        }
    }

    /**
     * Gets estimated position or null if not available yet.
     *
     * @return estimated position or null.
     */
    public P getEstimatedPosition() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2484
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2751
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS ||
                initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMa.getElementAtIndex(0);
        mInitialMyx = initialMa.getElementAtIndex(1);
        mInitialMzx = initialMa.getElementAtIndex(2);

        mInitialMxy = initialMa.getElementAtIndex(3);
        mInitialSy = initialMa.getElementAtIndex(4);
        mInitialMzy = initialMa.getElementAtIndex(5);

        mInitialMxz = initialMa.getElementAtIndex(6);
        mInitialMyz = initialMa.getElementAtIndex(7);
        mInitialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<? extends StandardDeviationFrameBodyKinematics> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body kinematics measurements taken at different
     *                     frames (positions, orientations and velocities).
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(
            final Collection<? extends StandardDeviationFrameBodyKinematics> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    @Override
    public KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 349
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 354
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 353
            final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<PreliminaryResult> innerEstimator =
                new PROSACRobustEstimator<>(
                        new PROSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROSACRobustKnownFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1347
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1315
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @param qualityScores quality scores corresponding to each sequence.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mSequences.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<PreliminaryResult> innerEstimator =
                new PROSACRobustEstimator<>(
                        new PROSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mSequences.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROSACRobustEasyGyroscopeCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3842
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3905
                commonAxisUsed, estimateGDependentCrossBiases, bias,
                initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<PreliminaryResult> innerEstimator =
                new PROSACRobustEstimator<>(
                        new PROSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROSACRobustKnownBiasTurntableGyroscopeCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3470
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3428
        mInitialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS ||
                initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMm.getElementAtIndex(0);
        mInitialMyx = initialMm.getElementAtIndex(1);
        mInitialMzx = initialMm.getElementAtIndex(2);

        mInitialMxy = initialMm.getElementAtIndex(3);
        mInitialSy = initialMm.getElementAtIndex(4);
        mInitialMzy = initialMm.getElementAtIndex(5);

        mInitialMxz = initialMm.getElementAtIndex(6);
        mInitialMyz = initialMm.getElementAtIndex(7);
        mInitialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<? extends StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body magnetic flux density measurements
     *                     taken at different frames (positions, orientations
     *                     and velocities).
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setMeasurements(
            final Collection<? extends StandardDeviationFrameBodyMagneticFluxDensity> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer.
     * When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer, false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mm matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for
     *                       accelerometer, gyroscope and magnetometer, false
     *                       otherwise.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    @Override
    public KnownFrameMagnetometerNonLinearLeastSquaresCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1679
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1697
        super(position, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        mGravityNorm = computeGravityNorm();

        final PROSACRobustEstimator<PreliminaryResult> innerEstimator =
                new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2129
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1444
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2336
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    public double[] getEstimatedHardIron() {
        return mEstimatedHardIron;
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where estimated magnetometer biases will be
     *               stored.
     * @return true if result instance was updated, false otherwise (when
     * estimation is not yet available).
     */
    public boolean getEstimatedHardIron(final double[] result) {
        if (mEstimatedHardIron != null) {
            System.arraycopy(mEstimatedHardIron, 0, result,
                    0, mEstimatedHardIron.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @return column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases.
     */
    public Matrix getEstimatedHardIronAsMatrix() {
        return mEstimatedHardIron != null ? Matrix.newFromArray(mEstimatedHardIron) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedHardIronAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedHardIron != null) {
            result.fromArray(mEstimatedHardIron);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return x coordinate of estimated magnetometer bias or null if not
     * available.
     */
    public Double getEstimatedHardIronX() {
        return mEstimatedHardIron != null ? mEstimatedHardIron[0] : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return y coordinate of estimated magnetometer bias or null if not
     * available.
     */
    public Double getEstimatedHardIronY() {
        return mEstimatedHardIron != null ? mEstimatedHardIron[1] : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return z coordinate of estimated magnetometer bias or null if not
     * available.
     */
    public Double getEstimatedHardIronZ() {
        return mEstimatedHardIron != null ? mEstimatedHardIron[2] : null;
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1780
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1854
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return mUseLinearCalibrator;
    }

    /**
     * Sepecifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(
            final boolean linearCalibratorUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mUseLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return mRefinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(
            final boolean preliminarySolutionRefined)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mRefinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(
            final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(
            final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator2D.java 2955
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator3D.java 2956
        final Point2D initialPosition = result.getEstimatedPosition();
        final double initialTransmittedPowerdBm =
                result.getEstimatedTransmittedPowerdBm();
        final double initialPathLossExponent = result.getEstimatedPathLossExponent();

        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mReadings.size();

            mInnerReadings.clear();

            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    //sample is inlier
                    mInnerReadings.add(mReadings.get(i));
                }
            }

            try {
                mInnerEstimator.setInitialPosition(initialPosition);
                mInnerEstimator.setInitialTransmittedPowerdBm(initialTransmittedPowerdBm);
                mInnerEstimator.setInitialPathLossExponent(initialPathLossExponent);
                mInnerEstimator.setTransmittedPowerEstimationEnabled(mTransmittedPowerEstimationEnabled);
                mInnerEstimator.setPositionEstimationEnabled(mPositionEstimationEnabled);
                mInnerEstimator.setPathLossEstimationEnabled(mPathLossEstimationEnabled);
                mInnerEstimator.setReadings(mInnerReadings);

                mInnerEstimator.estimate();

                final Matrix cov = mInnerEstimator.getEstimatedCovariance();
                if (mKeepCovariance && cov != null) {
                    //keep covariance
                    mCovariance = cov;

                    final int dims = getNumberOfDimensions();
                    int pos = 0;
                    if (mPositionEstimationEnabled) {
                        //position estimation enabled
                        final int d = dims - 1;
                        if (mEstimatedPositionCovariance == null) {
                            mEstimatedPositionCovariance = mCovariance.
                                    getSubmatrix(0, 0, d, d);
                        } else {
                            mCovariance.getSubmatrix(0, 0, d, d,
                                    mEstimatedPositionCovariance);
                        }
                        pos += dims;
                    } else {
                        //position estimation disabled
                        mEstimatedPositionCovariance = null;
                    }

                    if (mTransmittedPowerEstimationEnabled) {
                        //transmitted power estimation enabled
                        mEstimatedTransmittedPowerVariance = mCovariance.
                                getElementAt(pos, pos);
                        pos++;
                    } else {
                        //transmitted power estimation disabled
                        mEstimatedTransmittedPowerVariance = null;
                    }

                    if (mPathLossEstimationEnabled) {
                        //pathloss exponent estimation enabled
                        mEstimatedPathLossExponentVariance = mCovariance.
                                getElementAt(pos, pos);
                    } else {
                        //pathloss exponent estimation disabled
                        mEstimatedPathLossExponentVariance = null;
                    }
                } else {
                    mCovariance = null;
                    mEstimatedPositionCovariance = null;
                    mEstimatedTransmittedPowerVariance = null;
                    mEstimatedPathLossExponentVariance = null;
                }

                mEstimatedPosition = mInnerEstimator.getEstimatedPosition();
                mEstimatedTransmittedPowerdBm =
                        mInnerEstimator.getEstimatedTransmittedPowerdBm();
                mEstimatedPathLossExponent =
                        mInnerEstimator.getEstimatedPathLossExponent();
            } catch (final Exception e) {
                //refinement failed, so we return input value, and covariance
                //becomes unavailable
                mCovariance = null;
                mEstimatedPositionCovariance = null;
                mEstimatedTransmittedPowerVariance = null;
                mEstimatedPathLossExponentVariance = null;

                mEstimatedPosition = initialPosition;
                mEstimatedTransmittedPowerdBm = initialTransmittedPowerdBm;
                mEstimatedPathLossExponent = initialPathLossExponent;
            }
        } else {
            mCovariance = null;
            mEstimatedPositionCovariance = null;
            mEstimatedTransmittedPowerVariance = null;
            mEstimatedPathLossExponentVariance = null;

            mEstimatedPosition = initialPosition;
            mEstimatedTransmittedPowerdBm = initialTransmittedPowerdBm;
            mEstimatedPathLossExponent = initialPathLossExponent;
        }
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1780
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1259
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1854
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1318
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return mUseLinearCalibrator;
    }

    /**
     * Sepecifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(
            final boolean linearCalibratorUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mUseLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return mRefinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(
            final boolean preliminarySolutionRefined)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mRefinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(
            final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(
            final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1575
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 355
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Matrix> innerEstimator =
                new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<Matrix> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final Matrix currentEstimation,
                            final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 350
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1580
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 355
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 354
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<PreliminaryResult> innerEstimator =
                new PROSACRobustEstimator<>(
                        new PROSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROSACRobustKnownFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1575
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 355
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2061
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Matrix> innerEstimator =
                new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<Matrix> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final Matrix currentEstimation,
                            final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 350
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1580
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3843
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 355
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3906
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 354
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2058
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<PreliminaryResult> innerEstimator =
                new PROSACRobustEstimator<>(
                        new PROSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROSACRobustKnownFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2745
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2707
            final RobustEasyGyroscopeCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Gets minimum number of required sequences.
     *
     * @return minimum number of required sequences.
     */
    public int getMinimumRequiredSequences() {
        if (mCommonAxisUsed) {
            if (mEstimateGDependentCrossBiases) {
                return EasyGyroscopeCalibrator
                        .MINIMUM_SEQUENCES_COMMON_Z_AXIS_AND_CROSS_BIASES;
            } else {
                return EasyGyroscopeCalibrator.MINIMUM_SEQUENCES_COMMON_Z_AXIS;
            }
        } else {
            if (mEstimateGDependentCrossBiases) {
                return EasyGyroscopeCalibrator
                        .MINIMUM_SEQUENCES_GENERAL_AND_CROSS_BIASES;
            } else {
                return EasyGyroscopeCalibrator.MINIMUM_SEQUENCES_GENERAL;
            }
        }
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    public boolean isReady() {
        return mSequences != null
                && mSequences.size() >= getMinimumRequiredSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public abstract void calibrate() throws LockedException, NotReadyException,
            CalibrationException;

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2019
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1402
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator2D.java 2975
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator3D.java 2982
        final Point2D initialPosition = result.getEstimatedPosition();
        final double initialTransmittedPowerdBm =
                result.getEstimatedTransmittedPowerdBm();
        final double initialPathLossExponent = result.getEstimatedPathLossExponent();

        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mReadings.size();

            mInnerReadings.clear();

            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    //sample is inlier
                    mInnerReadings.add(mReadings.get(i));
                }
            }

            try {
                mInnerEstimator.setInitialPosition(initialPosition);
                mInnerEstimator.setInitialTransmittedPowerdBm(initialTransmittedPowerdBm);
                mInnerEstimator.setInitialPathLossExponent(initialPathLossExponent);
                mInnerEstimator.setTransmittedPowerEstimationEnabled(
                        mTransmittedPowerEstimationEnabled);
                mInnerEstimator.setPathLossEstimationEnabled(
                        mPathLossEstimationEnabled);
                mInnerEstimator.setReadings(mInnerReadings);
                mInnerEstimator.setUseReadingPositionCovariances(
                        mUseReadingPositionCovariances);

                mInnerEstimator.estimate();

                final Matrix cov = mInnerEstimator.getEstimatedCovariance();
                if (mKeepCovariance && cov != null) {
                    //keep covariance
                    mCovariance = cov;

                    final int dims = getNumberOfDimensions();
                    int pos = 0;

                    final int d = dims - 1;
                    if (mEstimatedPositionCovariance == null) {
                        mEstimatedPositionCovariance = mCovariance.
                                getSubmatrix(0, 0, d, d);
                    } else {
                        mCovariance.getSubmatrix(0, 0, d, d,
                                mEstimatedPositionCovariance);
                    }
                    pos += dims;

                    if (mTransmittedPowerEstimationEnabled) {
                        //transmitted power estimation enabled
                        mEstimatedTransmittedPowerVariance = mCovariance.
                                getElementAt(pos, pos);
                        pos++;
                    } else {
                        //transmitted power estimation disabled
                        mEstimatedTransmittedPowerVariance = null;
                    }

                    if (mPathLossEstimationEnabled) {
                        //pathloss exponent estimation enabled
                        mEstimatedPathLossExponentVariance = mCovariance.
                                getElementAt(pos, pos);
                    } else {
                        //pathloss exponent estimation disabled
                        mEstimatedPathLossExponentVariance = null;
                    }
                } else {
                    mCovariance = null;
                    mEstimatedPositionCovariance = null;
                    mEstimatedTransmittedPowerVariance = null;
                    mEstimatedPathLossExponentVariance = null;
                }

                mEstimatedPosition = mInnerEstimator.getEstimatedPosition();
                mEstimatedTransmittedPowerdBm =
                        mInnerEstimator.getEstimatedTransmittedPowerdBm();
                mEstimatedPathLossExponent =
                        mInnerEstimator.getEstimatedPathLossExponent();
            } catch (final Exception e) {
                //refinement failed, so we return input value, and covariance
                //becomes unavailable
                mCovariance = null;
                mEstimatedPositionCovariance = null;
                mEstimatedTransmittedPowerVariance = null;
                mEstimatedPathLossExponentVariance = null;

                mEstimatedPosition = initialPosition;
                mEstimatedTransmittedPowerdBm = initialTransmittedPowerdBm;
                mEstimatedPathLossExponent = initialPathLossExponent;
            }
        } else {
            mCovariance = null;
            mEstimatedPositionCovariance = null;
            mEstimatedTransmittedPowerVariance = null;
            mEstimatedPathLossExponentVariance = null;

            mEstimatedPosition = initialPosition;
            mEstimatedTransmittedPowerdBm = initialTransmittedPowerdBm;
            mEstimatedPathLossExponent = initialPathLossExponent;
        }
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2081
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2097
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    public int getMinimumRequiredMeasurements() {
        return mCommonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS :
                MINIMUM_MEASUREMENTS_GENERAL;
    }

    /**
     * Indicates whether calibrator is ready to start the estimator.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    public boolean isReady() {
        return mMeasurements != null
                && mMeasurements.size() >= getMinimumRequiredMeasurements()
                && mPosition != null && mYear != null;
    }

    /**
     * Indicates whether calibrator is currently running or no.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Gets Earth's magnetic model.
     *
     * @return Earth's magnetic model or null if not provided.
     */
    public WorldMagneticModel getMagneticModel() {
        return mMagneticModel;
    }

    /**
     * Sets Earth's magnetic model.
     *
     * @param magneticModel Earth's magnetic model to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMagneticModel(final WorldMagneticModel magneticModel)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMagneticModel = magneticModel;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    public Matrix getEstimatedMm() {
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator.java 521
com/irurueta/navigation/indoor/radiosource/RangingAndRssiRadioSourceEstimator.java 516
            final MixedRadioSourceEstimatorListener<S, P> listener) {
        this(readings, initialPosition, initialTransmittedPowerdBm, listener);
        mInitialPathLossExponent = initialPathLossExponent;
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPowerdBm() {
        return mInitialTransmittedPowerdBm;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @param initialTransmittedPowerdBm initial transmitted power to start the
     *                                   estimation of radio source transmitted
     *                                   power.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialTransmittedPowerdBm(final Double initialTransmittedPowerdBm)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialTransmittedPowerdBm = initialTransmittedPowerdBm;
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPower() {
        return mInitialTransmittedPowerdBm != null ?
                Utils.dBmToPower(mInitialTransmittedPowerdBm) : null;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @param initialTransmittedPower initial transmitted power to start the
     *                                estimation of radio source transmitted power.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setInitialTransmittedPower(final Double initialTransmittedPower)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (initialTransmittedPower != null) {
            if (initialTransmittedPower < 0.0) {
                throw new IllegalArgumentException();
            }
            mInitialTransmittedPowerdBm = Utils.powerTodBm(
                    initialTransmittedPower);
        } else {
            mInitialTransmittedPowerdBm = null;
        }
    }

    /**
     * Indicates whether transmitted power estimation is enabled or not.
     *
     * @return true if transmitted power estimation is enabled, false otherwise.
     */
    public boolean isTransmittedPowerEstimationEnabled() {
        return mTransmittedPowerEstimationEnabled;
    }

    /**
     * Specifies whether transmitted power estimation is enabled or not.
     *
     * @param transmittedPowerEstimationEnabled true if transmitted power estimation is enabled,
     *                                          false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setTransmittedPowerEstimationEnabled(
            final boolean transmittedPowerEstimationEnabled)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mTransmittedPowerEstimationEnabled = transmittedPowerEstimationEnabled;
    }

    /**
     * Gets initial position to start the estimation of radio source position.
     * If not defined, centroid of provided readings will be used.
     * <p>
     * If position estimation is enabled, estimation will start at this value
     * and will converge to the most appropriate value.
     * If position estimation is disabled, this value will be assumed to
     * be exact and the estimated position will be equal to this value.
     *
     * @return initial position to start the estimation of radio source position.
     */
    public P getInitialPosition() {
        return mInitialPosition;
    }

    /**
     * Sets initial position to start the estimation of radio source position.
     * If not defined, centroid of provided fingerprints will be used.
     * <p>
     * If position estimation is enabled, estimation will start at this value
     * and will converge to the most appropriate value.
     * If position estimation is disabled, this value will be assumed to
     * be exact and the estimated position will be equal to this value.
     *
     * @param initialPosition initial position to start the estimation of radio
     *                        source position.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialPosition(final P initialPosition) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialPosition = initialPosition;
    }

    /**
     * Gets initial exponent typically used on free space for path loss propagation
     * in terms of distance.
     * On different environments path loss exponent might have different value:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     * <p>
     * If path loss exponent estimation is enabled, estimation will start at this
     * value and will converge to the most appropriate value.
     * If path loss exponent estimation is disabled, this value will be assumed
     * to be exact and the estimated path loss exponent will be equal to this
     * value.
     *
     * @return initial path loss exponent.
     */
    public double getInitialPathLossExponent() {
        return mInitialPathLossExponent;
    }

    /**
     * Sets initial exponent typically used on free space for path loss propagation
     * in terms of distance.
     * On different environments path loss exponent might have different value:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     * <p>
     * If path loss exponent estimation is enabled, estimation will start at this
     * value and will converge to the most appropriate value.
     * If path loss exponent estimation is disabled, this value will be assumed
     * to be exact and the estimated path loss exponent will be equal to this
     * value.
     *
     * @param initialPathLossExponent initial path loss exponent.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialPathLossExponent(final double initialPathLossExponent)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialPathLossExponent = initialPathLossExponent;
    }

    /**
     * Indicates whether path loss estimation is enabled or not.
     *
     * @return true if path loss estimation is enabled, false otherwise.
     */
    public boolean isPathLossEstimationEnabled() {
        return mPathLossEstimationEnabled;
    }

    /**
     * Specifies whether path loss estimation is enabled or not.
     *
     * @param pathLossEstimationEnabled true if path loss estimation is enabled,
     *                                  false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setPathLossEstimationEnabled(final boolean pathLossEstimationEnabled)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mPathLossEstimationEnabled = pathLossEstimationEnabled;
    }

    /**
     * Indicates whether position covariances of readings must be taken into account to increase
     * the amount of standard deviation of each ranging measure by the amount of position standard
     * deviation assuming that both measures are statistically independent.
     *
     * @return true to take into account reading position covariances, false otherwise.
     */
    public boolean getUseReadingPositionCovariance() {
        return mUseReadingPositionCovariances;
    }

    /**
     * Specifies whether position covariances of readings must be taken into account to increase
     * the amount of standard deviation of each ranging measure by the amount of position standard
     * deviation assuming that both measures are statistically independent.
     *
     * @param useReadingPositionCovariances true to take into account reading position covariances, false
     *                                      otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setUseReadingPositionCovariances(
            final boolean useReadingPositionCovariances) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mUseReadingPositionCovariances = useReadingPositionCovariances;
    }

    /**
     * Indicates whether an homogeneous linear solver is used to estimate an initial
     * position for the internal ranging radio source estimator.
     *
     * @return true if homogeneous linear solver is used, false if an inhomogeneous linear
     * one is used instead.
     */
    public boolean isHomogeneousRangingLinearSolverUsed() {
        return mUseHomogeneousRangingLinearSolver;
    }

    /**
     * Specifies whether an homogeneous linear solver is used to estimate an initial
     * position for the internal ranging radio source estimator.
     *
     * @param useHomogeneousLinearSolver true if homogeneous linear solver is used, false
     *                                   if an inhomogeneous linear one is used instead.
     * @throws LockedException if estimator is locked.
     */
    public void setHomogeneousRangingLinearSolverUsed(
            final boolean useHomogeneousLinearSolver) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        mUseHomogeneousRangingLinearSolver = useHomogeneousLinearSolver;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3245
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4736
        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double m11 = result[0];
        final double m21 = result[1];
        final double m31 = result[2];

        final double m12 = result[3];
        final double m22 = result[4];
        final double m32 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final double g11 = result[9];
        final double g21 = result[10];
        final double g31 = result[11];

        final double g12 = result[12];
        final double g22 = result[13];
        final double g32 = result[14];

        final double g13 = result[15];
        final double g23 = result[16];
        final double g33 = result[17];

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, m21);
        m.setElementAtIndex(2, m31);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, m32);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        final Matrix g = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        g.setElementAtIndex(0, g11);
        g.setElementAtIndex(1, g21);
        g.setElementAtIndex(2, g31);

        g.setElementAtIndex(3, g12);
        g.setElementAtIndex(4, g22);
        g.setElementAtIndex(5, g32);

        g.setElementAtIndex(6, g13);
        g.setElementAtIndex(7, g23);
        g.setElementAtIndex(8, g33);

        setResult(m, g);
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and G-dependent cross biases
     * are ignored.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxis() throws AlgebraException,
            FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 989
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 964
        mInitialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMm(final Matrix initialMm)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS ||
                initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMm.getElementAtIndex(0);
        mInitialMyx = initialMm.getElementAtIndex(1);
        mInitialMzx = initialMm.getElementAtIndex(2);

        mInitialMxy = initialMm.getElementAtIndex(3);
        mInitialSy = initialMm.getElementAtIndex(4);
        mInitialMzy = initialMm.getElementAtIndex(5);

        mInitialMxz = initialMm.getElementAtIndex(6);
        mInitialMyz = initialMm.getElementAtIndex(7);
        mInitialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets a list of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    public List<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets a list of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body magnetic flux density measurements
     *                     taken at different frames (positions, orientations
     *                     and velocities).
     * @throws LockedException if estimator is currently running.
     */
    public void setMeasurements(
            final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer.
     * When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer, false otherwise.
     */
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mm matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for
     *                       accelerometer, gyroscope and magnetometer, false
     *                       otherwise.
     * @throws LockedException if estimator is currently running.
     */
    public void setCommonAxisUsed(final boolean commonAxisUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    public RobustKnownFrameMagnetometerCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3008
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2798
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
        return mEstimatedChiSq;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxisAndGDependentCrossBiases()
            throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
        // The gyroscope model is
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Where Mg is upper triangular

        // For convergence purposes of the Levenberg-Marquardt algorithm, we
        // take common factor M = I + Mg

        // and the gyroscope model can be better expressed as:

        // Ωmeas = M*(Ωtrue + b + G * ftrue)

        // where:
        // bg = M*b --> b = M^-1*bg
        // Gg = M*G --> G = M^-1*Gg

        // Hence
        // Ωmeas - M*b - M*G*ftrue = M*Ωtrue
        // M^-1 * (Ωmeas - M*b - M*G*ftrue) = Ωtrue

        // Notice that M is upper diagonal because Mg is upper diagonal
        // when common axis is assumed

        final GradientEstimator gradientEstimator = new GradientEstimator(
                new MultiDimensionFunctionEvaluatorListener() {
                    @Override
                    public double evaluate(final double[] params)
                            throws EvaluationException {
                        return evaluateCommonAxisWithGDependentCrossBiases(mI, params);
                    }
                });

        final Matrix initialM = Matrix.identity(
                BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        initialM.add(getInitialMg());

        // Force initial M to be upper diagonal
        initialM.setElementAt(1, 0, 0.0);
        initialM.setElementAt(2, 0, 0.0);
        initialM.setElementAt(2, 1, 0.0);

        final Matrix invInitialM = Utils.inverse(initialM);
        final Matrix initialBg = getInitialBiasAsMatrix();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1610
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1089
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS ||
                initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMa.getElementAtIndex(0);
        mInitialMyx = initialMa.getElementAtIndex(1);
        mInitialMzx = initialMa.getElementAtIndex(2);

        mInitialMxy = initialMa.getElementAtIndex(3);
        mInitialSy = initialMa.getElementAtIndex(4);
        mInitialMzy = initialMa.getElementAtIndex(5);

        mInitialMxz = initialMa.getElementAtIndex(6);
        mInitialMyz = initialMa.getElementAtIndex(7);
        mInitialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    public List<StandardDeviationFrameBodyKinematics> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body kinematics measurements taken at different
     *                     frames (positions, orientations and velocities).
     * @throws LockedException if calibrator is currently running.
     */
    public void setMeasurements(
            final List<StandardDeviationFrameBodyKinematics> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public RobustKnownBiasAndFrameAccelerometerCalibratorListener getListener() {
File Line
com/irurueta/navigation/indoor/Utils.java 503
com/irurueta/navigation/indoor/Utils.java 1203
com/irurueta/navigation/indoor/Utils.java 2161
    public static MultivariateNormalDist propagateVariancesToRssiVarianceFirstOrderNonLinear3D(
            final double fingerprintRssi, final double pathLossExponent,
            final Point3D fingerprintPosition, final Point3D radioSourcePosition,
            final Point3D estimatedPosition,
            final Double fingerprintRssiVariance,
            final Double pathLossExponentVariance,
            final Matrix fingerprintPositionCovariance,
            final Matrix radioSourcePositionCovariance,
            final Matrix estimatedPositionCovariance) throws IndoorException {

        if (fingerprintPosition == null || radioSourcePosition == null ||
                estimatedPosition == null) {
            return null;
        }

        //1st order Taylor expression of received power in 3D:
        //Pr(pi) = Pr(p1)
        //  - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
        //  - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
        //  - 10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1)
        //where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2

        final double x1 = fingerprintPosition.getInhomX();
        final double y1 = fingerprintPosition.getInhomY();
        final double z1 = fingerprintPosition.getInhomZ();

        final double xa = radioSourcePosition.getInhomX();
        final double ya = radioSourcePosition.getInhomY();
        final double za = radioSourcePosition.getInhomZ();

        final double xi = estimatedPosition.getInhomX();
        final double yi = estimatedPosition.getInhomY();
        final double zi = estimatedPosition.getInhomZ();

        final double[] mean = new double[]{
                fingerprintRssi, pathLossExponent, x1, y1, z1, xa, ya, za, xi, yi, zi
        };
        final Matrix covariance = Matrix.diagonal(new double[]{
                fingerprintRssiVariance != null ? fingerprintRssiVariance : 0.0,
                pathLossExponentVariance != null ? pathLossExponentVariance : 0.0,
                0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
        });

        if (fingerprintPositionCovariance != null &&
                fingerprintPositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH &&
                fingerprintPositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {

            covariance.setSubmatrix(2, 2,
                    4, 4,
                    fingerprintPositionCovariance);
        }

        if (radioSourcePositionCovariance != null &&
                radioSourcePositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH &&
                radioSourcePositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
            covariance.setSubmatrix(5, 5,
                    7, 7,
                    radioSourcePositionCovariance);
        }

        if (estimatedPositionCovariance != null &&
                estimatedPositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH &&
                estimatedPositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
            covariance.setSubmatrix(8, 8,
                    10, 10,
                    estimatedPositionCovariance);
        }

        try {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 11854
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 12440
                bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener, DEFAULT_ROBUST_METHOD);
    }


    /**
     * Computes error of a preliminary result respect a given measurement.
     *
     * @param measurement       a measurement.
     * @param preliminaryResult a preliminary result.
     * @return computed error.
     */
    protected double computeError(
            final StandardDeviationBodyKinematics measurement,
            final PreliminaryResult preliminaryResult) {
        // We know that measured angular rate is:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Hence:
        // [Ωmeasx] = [bx] + ( [1   0   0] + [sx    mxy    mxz]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0   1   0]   [myx   sy     myz]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0   0   1]   [mzx   mzy    sz ]  [Ωtruez]   [g31   g32   g33][ftruez]

        final BodyKinematics measuredKinematics = measurement.getKinematics();

        final double[] specificForce = new double[]{
                measuredKinematics.getFx(),
                measuredKinematics.getFy(),
                measuredKinematics.getFz()
        };

        try {
            final double[] axis1 = ArrayUtils.normalizeAndReturnNew(specificForce);
            final Quaternion rot1 = new Quaternion(axis1, 0.0);

            final CoordinateTransformation nedC1 = new CoordinateTransformation(
                    rot1.asInhomogeneousMatrix(), FrameType.BODY_FRAME,
                    FrameType.LOCAL_NAVIGATION_FRAME);

            final NEDPosition nedPosition = getNedPosition();
            final NEDFrame nedFrame1 = new NEDFrame(nedPosition, nedC1);
            final ECEFFrame ecefFrame1 = NEDtoECEFFrameConverter
                    .convertNEDtoECEFAndReturnNew(nedFrame1);
            double timeInterval = mTimeInterval;
            double angleIncrement = mTurntableRotationRate * timeInterval;
            if (Math.abs(angleIncrement) > Math.PI / 2.0) {
                // angle = rot_rate * interval
                // rot_rate * interval / x = angle / x

                // if we want angle / x = pi / 2, then:
                final double x = Math.abs(angleIncrement) / (Math.PI / 2.0);
                timeInterval /= x;
                angleIncrement = mTurntableRotationRate * timeInterval;
            }
            final Rotation3D rot = new AxisRotation3D(axis1, angleIncrement);
            final Rotation3D rot2 = rot1.combineAndReturnNew(rot);
            final CoordinateTransformation nedC2 =
                    new CoordinateTransformation(rot2.asInhomogeneousMatrix(), FrameType.BODY_FRAME,
                            FrameType.LOCAL_NAVIGATION_FRAME);

            final NEDFrame nedFrame2 = new NEDFrame(nedPosition, nedC2);
            final ECEFFrame ecefFrame2 = NEDtoECEFFrameConverter
                    .convertNEDtoECEFAndReturnNew(nedFrame2);

            final BodyKinematics expectedKinematics = ECEFKinematicsEstimator
                    .estimateKinematicsAndReturnNew(timeInterval, ecefFrame2,
                            ecefFrame1);

            final double angularRateMeasX1 = measuredKinematics.getAngularRateX();
            final double angularRateMeasY1 = measuredKinematics.getAngularRateY();
            final double angularRateMeasZ1 = measuredKinematics.getAngularRateZ();

            final double angularRateTrueX = expectedKinematics.getAngularRateX();
            final double angularRateTrueY = expectedKinematics.getAngularRateY();
            final double angularRateTrueZ = expectedKinematics.getAngularRateZ();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            final Matrix mg = preliminaryResult.mEstimatedMg;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3109
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3436
            mRunning = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return mEstimatedChiSq;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {

        // The gyroscope model is:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // where myx = mzx = mzy = 0

        // Hence:
        // [Ωmeasx] = [bx] + ( [1   0   0] + [sx    mxy    mxz]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0   1   0]   [0     sy     myz]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0   0   1]   [0     0      sz ]  [Ωtruez]   [g31   g32   g33][ftruez]

        // [Ωmeasx] = [bx] + ( [1+sx  mxy    mxz ]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0     1+sy   myz ]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0     0      1+sz]  [Ωtruez]   [g31   g32   g33][ftruez]

        // Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myz, g11, g12, g13, g21, g22, g23, g31, g32, g33
        // Reordering:
        // Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Ωmeasx - Ωtruex - bx = sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy - Ωtruey - by = sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz - Ωtruez - bz = sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // [Ωtruex  0       0       Ωtruey  Ωtruez  0       ftruex  ftruey  ftruez  0       0       0       0       0       0     ][sx ] =  [Ωmeasx - Ωtruex - bx]
        // [0       Ωtruey  0       0       0       Ωtruez  0       0       0       ftruex  ftruey  ftruez  0       0       0     ][sy ]    [Ωmeasy - Ωtruey - by]
        // [0       0       Ωtruez  0       0       0       0       0       0       0       0       0       ftruex  ftruey  ftruez][sz ]    [Ωmeasz - Ωtruez - bz]
        //                                                                                                                         [mxy]
        //                                                                                                                         [mxz]
        //                                                                                                                         [myz]
        //                                                                                                                         [g11]
        //                                                                                                                         [g12]
        //                                                                                                                         [g13]
        //                                                                                                                         [g21]
        //                                                                                                                         [g22]
        //                                                                                                                         [g23]
        //                                                                                                                         [g31]
        //                                                                                                                         [g32]
        //                                                                                                                         [g33]

        mFitter.setFunctionEvaluator(
                new LevenbergMarquardtMultiVariateFunctionEvaluator() {
                    @Override
                    public int getNumberOfDimensions() {
                        // Input points are true angular rate + specific force coordinates
                        return 2 * BodyKinematics.COMPONENTS;
                    }

                    @Override
                    public int getNumberOfVariables() {
                        // The multivariate function returns the components of measured angular rate
                        return BodyKinematics.COMPONENTS;
                    }

                    @Override
                    public double[] createInitialParametersArray() {
                        final double[] initial = new double[COMMON_Z_AXIS_UNKNOWNS];

                        initial[0] = mInitialSx;
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator3D.java 836
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator3D.java 850
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Solution<Point3D>> innerEstimator =
                new PROMedSRobustEstimator<>(
                        new PROMedSRobustEstimatorListener<Solution<Point3D>>() {

                            @Override
                            public double[] getQualityScores() {
                                return mQualityScores;
                            }

                            @Override
                            public double getThreshold() {
                                return mStopThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point3D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point3D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return PROMedSRobustRangingAndRssiRadioSourceEstimator3D.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3865
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3595
        for (final BodyKinematicsSequence<StandardDeviationTimedBodyKinematics> sequence : mSequences) {
            mFixedSequences.add(new BodyKinematicsSequence<>(sequence));
        }

        mAccelerationFixer.setBias(ba);
        mAccelerationFixer.setCrossCouplingErrors(ma);

        int i = 0;
        for (final BodyKinematicsSequence<StandardDeviationTimedBodyKinematics> sequence : mSequences) {
            // sequence mean accelerometer samples of previous static
            // period will need to be fixed using accelerometer calibration
            // parameters
            measuredBeforeF[0] = sequence.getBeforeMeanFx();
            measuredBeforeF[1] = sequence.getBeforeMeanFy();
            measuredBeforeF[2] = sequence.getBeforeMeanFz();
            mAccelerationFixer.fix(measuredBeforeF, fixedBeforeF);

            measuredAfterF[0] = sequence.getAfterMeanFx();
            measuredAfterF[1] = sequence.getAfterMeanFy();
            measuredAfterF[2] = sequence.getAfterMeanFz();
            mAccelerationFixer.fix(measuredAfterF, fixedAfterF);

            // because we are only interested in gravity direction, we
            // normalize these vectors, so that gravity becomes independent
            // of current Earth position.
            ArrayUtils.normalize(fixedBeforeF);
            ArrayUtils.normalize(fixedAfterF);

            x.setSubmatrix(i, 0, i, 2,
                    fixedBeforeF);
            x.setSubmatrix(i, 3, i, 5,
                    fixedAfterF);

            y[i] = 0.0;

            standardDeviations[i] = computeAverageAngularRateStandardDeviation(
                    sequence);
            i++;
        }

        mFitter.setInputData(x, y, standardDeviations);
    }

    /**
     * Computes average angular rate standard deviation for measurements
     * in provided sequence.
     *
     * @param sequence a sequence.
     * @return average angular rate standard deviation expressed in radians
     * per second (rad/s).
     */
    private static double computeAverageAngularRateStandardDeviation(
            final BodyKinematicsSequence<StandardDeviationTimedBodyKinematics> sequence) {
        final List<StandardDeviationTimedBodyKinematics> items = sequence.getSortedItems();
        final double size = items.size();

        double result = 0.0;
        for (final StandardDeviationTimedBodyKinematics item : items) {
            result += item.getAngularRateStandardDeviation() / size;
        }

        return result;
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(),
                acceleration.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second.
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value.
     */
    private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
        return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(),
                angularSpeed.getUnit(), AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Makes proper conversion of internal cross-coupling, bias and g-dependent
     * cross bias matrices.
     *
     * @param m internal scaling and cross-coupling matrix.
     * @param b internal bias matrix.
     * @param g internal g-dependent cross bias matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m, final Matrix b, final Matrix g)
File Line
com/irurueta/navigation/indoor/position/PROSACRobustMixedPositionEstimator2D.java 365
com/irurueta/navigation/indoor/position/PROSACRobustRangingAndRssiPositionEstimator2D.java 370
com/irurueta/navigation/indoor/position/PROSACRobustRangingPositionEstimator2D.java 366
com/irurueta/navigation/indoor/position/PROSACRobustRssiPositionEstimator2D.java 368
            final RobustMixedPositionEstimatorListener<Point2D> listener) {
        this(sources, fingerprint, listener);
        internalSetSourceQualityScores(sourceQualityScores);
        internalSetFingerprintReadingsQualityScores(fingerprintReadingQualityScores);
    }

    /**
     * Returns quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each radio source.
     */
    public double[] getSourceQualityScores() {
        return mSourceQualityScores;
    }

    /**
     * Sets quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the radio source.
     *
     * @param sourceQualityScores quality scores corresponding to each radio source.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setSourceQualityScores(final double[] sourceQualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSourceQualityScores(sourceQualityScores);
    }

    /**
     * Gets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each reading within provided
     * fingerprint.
     */
    public double[] getFingerprintReadingsQualityScores() {
        return mFingerprintReadingsQualityScores;
    }

    /**
     * Sets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @param fingerprintReadingsQualityScores quality scores corresponding to each
     *                                         reading within provided fingerprint.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return ((PROSACRobustLateration2DSolver) mLaterationSolver).
                getThreshold();
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        ((PROSACRobustLateration2DSolver) mLaterationSolver).
                setThreshold(threshold);
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return ((PROSACRobustLateration2DSolver) mLaterationSolver).
                isComputeAndKeepInliersEnabled();
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        ((PROSACRobustLateration2DSolver) mLaterationSolver).
                setComputeAndKeepInliersEnabled(computeAndKeepInliers);
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return ((PROSACRobustLateration2DSolver) mLaterationSolver).
                isComputeAndKeepResiduals();
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        ((PROSACRobustLateration2DSolver) mLaterationSolver).
                setComputeAndKeepResidualsEnabled(computeAndKeepResiduals);
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Initializes robust lateration solver.
     */
    private void init() {
        mLaterationSolver = new PROSACRobustLateration2DSolver(
                mTrilaterationSolverListener);
    }

    /**
     * Sets quality scores corresponding to each provided located radio source.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param sourceQualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetSourceQualityScores(final double[] sourceQualityScores) {
        if (sourceQualityScores == null ||
                sourceQualityScores.length < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mSourceQualityScores = sourceQualityScores;

        buildPositionsDistancesDistanceStandardDeviationsAndQualityScores();
    }

    /**
     * Sets quality scores corresponding to each provided reading within provided
     * fingerprint.
     * This method is used internally and does not check whether instance is locked
     * or not.
     *
     * @param fingerprintReadingsQualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores lengt is
     *                                  smaller than 3 samples.
     */
    private void internalSetFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) {
        if (fingerprintReadingsQualityScores == null ||
                fingerprintReadingsQualityScores.length < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mFingerprintReadingsQualityScores = fingerprintReadingsQualityScores;

        buildPositionsDistancesDistanceStandardDeviationsAndQualityScores();
    }
}
File Line
com/irurueta/navigation/indoor/position/PROSACRobustMixedPositionEstimator3D.java 366
com/irurueta/navigation/indoor/position/PROSACRobustRangingAndRssiPositionEstimator3D.java 370
com/irurueta/navigation/indoor/position/PROSACRobustRangingPositionEstimator3D.java 366
com/irurueta/navigation/indoor/position/PROSACRobustRssiPositionEstimator3D.java 368
            final RobustMixedPositionEstimatorListener<Point3D> listener) {
        this(sources, fingerprint, listener);
        internalSetSourceQualityScores(sourceQualityScores);
        internalSetFingerprintReadingsQualityScores(fingerprintReadingQualityScores);
    }

    /**
     * Returns quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each radio source.
     */
    public double[] getSourceQualityScores() {
        return mSourceQualityScores;
    }

    /**
     * Sets quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the radio source.
     *
     * @param sourceQualityScores quality scores corresponding to each radio source.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setSourceQualityScores(final double[] sourceQualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSourceQualityScores(sourceQualityScores);
    }

    /**
     * Gets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each reading within provided
     * fingerprint.
     */
    public double[] getFingerprintReadingsQualityScores() {
        return mFingerprintReadingsQualityScores;
    }

    /**
     * Sets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @param fingerprintReadingsQualityScores quality scores corresponding to each
     *                                         reading within provided fingerprint.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return ((PROSACRobustLateration3DSolver) mLaterationSolver).
                getThreshold();
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        ((PROSACRobustLateration3DSolver) mLaterationSolver).
                setThreshold(threshold);
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return ((PROSACRobustLateration3DSolver) mLaterationSolver).
                isComputeAndKeepInliersEnabled();
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        ((PROSACRobustLateration3DSolver) mLaterationSolver).
                setComputeAndKeepInliersEnabled(computeAndKeepInliers);
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return ((PROSACRobustLateration3DSolver) mLaterationSolver).
                isComputeAndKeepResiduals();
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        ((PROSACRobustLateration3DSolver) mLaterationSolver).
                setComputeAndKeepResidualsEnabled(computeAndKeepResiduals);
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Initializes robust lateration solver.
     */
    private void init() {
        mLaterationSolver = new PROSACRobustLateration3DSolver(
                mTrilaterationSolverListener);
    }

    /**
     * Sets quality scores corresponding to each provided located radio source.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param sourceQualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetSourceQualityScores(final double[] sourceQualityScores) {
        if (sourceQualityScores == null ||
                sourceQualityScores.length < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mSourceQualityScores = sourceQualityScores;

        buildPositionsDistancesDistanceStandardDeviationsAndQualityScores();
    }

    /**
     * Sets quality scores corresponding to each provided reading within provided
     * fingerprint.
     * This method is used internally and does not check whether instance is locked
     * or not.
     *
     * @param fingerprintReadingsQualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores lengt is
     *                                  smaller than 3 samples.
     */
    private void internalSetFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) {
        if (fingerprintReadingsQualityScores == null ||
                fingerprintReadingsQualityScores.length < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mFingerprintReadingsQualityScores = fingerprintReadingsQualityScores;

        buildPositionsDistancesDistanceStandardDeviationsAndQualityScores();
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3090
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4551
        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double m11 = result[0];

        final double m12 = result[1];
        final double m22 = result[2];

        final double m13 = result[3];
        final double m23 = result[4];
        final double m33 = result[5];

        final double g11 = result[6];
        final double g21 = result[7];
        final double g31 = result[8];

        final double g12 = result[9];
        final double g22 = result[10];
        final double g32 = result[11];

        final double g13 = result[12];
        final double g23 = result[13];
        final double g33 = result[14];

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, 0.0);
        m.setElementAtIndex(2, 0.0);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        final Matrix g = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        g.setElementAtIndex(0, g11);
        g.setElementAtIndex(1, g21);
        g.setElementAtIndex(2, g31);

        g.setElementAtIndex(3, g12);
        g.setElementAtIndex(4, g22);
        g.setElementAtIndex(5, g32);

        g.setElementAtIndex(6, g13);
        g.setElementAtIndex(7, g23);
        g.setElementAtIndex(8, g33);

        setResult(m, g);
    }

    /**
     * Internal method to perform general calibration when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneralAndGDependentCrossBiases()
            throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7786
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2783
                    mEstimatedMa.getElementAt(i, i) - 1.0);
        }

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters for the general case.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the general purpose case.
     *               Must have length 12.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneral(final double[] params) throws EvaluationException {
        final double bx = params[0];
        final double by = params[1];
        final double bz = params[2];

        final double m11 = params[3];
        final double m21 = params[4];
        final double m31 = params[5];

        final double m12 = params[6];
        final double m22 = params[7];
        final double m32 = params[8];

        final double m13 = params[9];
        final double m23 = params[10];
        final double m33 = params[11];

        return evaluate(bx, by, bz, m11, m21, m31, m12, m22, m32,
                m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters when common z-axis is assumed.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the common z-axis case.
     *               Must have length 9.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateCommonAxis(final double[] params) throws EvaluationException {
        final double bx = params[0];
        final double by = params[1];
        final double bz = params[2];

        final double m11 = params[3];

        final double m12 = params[4];
        final double m22 = params[5];

        final double m13 = params[6];
        final double m23 = params[7];
        final double m33 = params[8];

        return evaluate(bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0,
                m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param bx  x-coordinate of bias.
     * @param by  y-coordinate of bias.
     * @param bz  z-coordinate of bias.
     * @param m11 element 1,1 of cross-coupling error matrix.
     * @param m21 element 2,1 of cross-coupling error matrix.
     * @param m31 element 3,1 of cross-coupling error matrix.
     * @param m12 element 1,2 of cross-coupling error matrix.
     * @param m22 element 2,2 of cross-coupling error matrix.
     * @param m32 element 3,2 of cross-coupling error matrix.
     * @param m13 element 1,3 of cross-coupling error matrix.
     * @param m23 element 2,3 of cross-coupling error matrix.
     * @param m33 element 3,3 of cross-coupling error matrix.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluate(final double bx, final double by, final double bz,
                            final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33)
            throws EvaluationException {

        // fmeas = M*(ftrue + b)

        // ftrue = M^-1*fmeas - b

        try {
            if (mFmeas == null) {
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator2D.java 2864
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator2D.java 2848
        final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings = getReadings();
        if (readings == null || readings.isEmpty()) {
            return null;
        }
        final S source = readings.get(0).getSource();

        final Point2D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        final Double transmittedPowerVariance =
                getEstimatedTransmittedPowerVariance();
        final Double transmittedPowerStandardDeviation = transmittedPowerVariance != null ?
                Math.sqrt(transmittedPowerVariance) : null;

        final Double pathlossExponentVariance =
                getEstimatedPathLossExponentVariance();
        final Double pathlossExponentStandardDeviation = pathlossExponentVariance != null ?
                Math.sqrt(pathlossExponentVariance) : null;

        if (source instanceof WifiAccessPoint) {
            final WifiAccessPoint accessPoint = (WifiAccessPoint) source;
            return new WifiAccessPointWithPowerAndLocated2D(accessPoint.getBssid(),
                    source.getFrequency(), accessPoint.getSsid(),
                    getEstimatedTransmittedPowerdBm(),
                    transmittedPowerStandardDeviation,
                    getEstimatedPathLossExponent(),
                    pathlossExponentStandardDeviation,
                    estimatedPosition,
                    estimatedPositionCovariance);
        } else if (source instanceof Beacon) {
            final Beacon beacon = (Beacon) source;
            return new BeaconWithPowerAndLocated2D(beacon.getIdentifiers(),
                    getEstimatedTransmittedPowerdBm(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(),
                    getEstimatedPathLossExponent(),
                    transmittedPowerStandardDeviation,
                    pathlossExponentStandardDeviation,
                    estimatedPosition, estimatedPositionCovariance);
        } else {
            return null;
        }
    }

    /**
     * Solves preliminar solution for a subset of samples.
     *
     * @param samplesIndices indices of subset samples.
     * @param solutions      instance where solution will be stored.
     */
    @Override
    protected void solvePreliminarSolutions(
            final int[] samplesIndices,
            final List<Solution<Point2D>> solutions) {

        try {
            int index;

            mInnerReadings.clear();
            for (final int samplesIndice : samplesIndices) {
                index = samplesIndice;
                mInnerReadings.add(mReadings.get(index));
            }

            //initial transmitted power and position might or might not be available
            mInnerEstimator.setInitialTransmittedPowerdBm(
                    mInitialTransmittedPowerdBm);
            mInnerEstimator.setInitialPosition(mInitialPosition);
            mInnerEstimator.setInitialPathLossExponent(mInitialPathLossExponent);

            mInnerEstimator.setTransmittedPowerEstimationEnabled(
                    mTransmittedPowerEstimationEnabled);
            mInnerEstimator.setPathLossEstimationEnabled(mPathLossEstimationEnabled);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2728
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3176
            mRunning = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return mEstimatedChiSq;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // Hence:
        //  [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        //  [fmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [ftruey]
        //  [fmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [ftruez]

        // where myx = mzx = mzy = 0

        // Hence:
        //  [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        //  [fmeasy] = [by]     [0  1   0]   [0     sy  myz]    [ftruey]
        //  [fmeasz] = [bz]     [0  0   1]   [0     0   sz ]    [ftruez]

        //  [fmeasx] = [bx] +   [1+sx   mxy     mxz ][ftruex]
        //  [fmeasy]   [by]     [0      1+sy    myz ][ftruey]
        //  [fmeasz]   [bz]     [0      0       1+sz][ftruez]

        //  fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + (1+sy) * ftruey + myz * ftruez
        //  fmeasz = bz + (1+sz) * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myz
        // Reordering:
        //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + ftruey + sy * ftruey + myz * ftruez
        //  fmeasz = bz + ftruez + sz * ftruez

        //  fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy - ftruey - by = sy * ftruey + myz * ftruez
        //  fmeasz - ftruez - bz = sz * ftruez

        // [ftruex  0       0       ftruey  ftruez  0     ][sx ] = [fmeasx - ftruex - bx]
        // [0       ftruey  0       0       0       ftruez][sy ]   [fmeasy - ftruey - by]
        // [0       0       ftruez  0       0       0     ][sz ]   [fmeasz - ftruez - bz]
        //                                                 [mxy]
        //                                                 [mxz]
        //                                                 [myz]

        mFitter.setFunctionEvaluator(new LevenbergMarquardtMultiVariateFunctionEvaluator() {
            @Override
            public int getNumberOfDimensions() {
                // Input points are true specific force coordinates
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public int getNumberOfVariables() {
                // The multivariate function returns the components of measured specific force
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final double[] initial = new double[COMMON_Z_AXIS_UNKNOWNS];

                initial[0] = mInitialSx;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1284
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1343
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1206
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return mUseLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mUseLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return mRefinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(
            final boolean preliminarySolutionRefined) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mRefinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(
            final float progressDelta) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(
            final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(
            final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(
            final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(
            final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3844
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3704
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return mEstimatedChiSq;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     * @throws IOException                              if world magnetic model cannot be loaded.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException, IOException {
        // The accelerometer model is:
        // mBmeas = ba + (I + Ma) * mBtrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // mBmeas = ba + (I + Ma) * mBtrue

        // Hence:
        //  [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        //  [mBmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [mBtruey]
        //  [mBmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [mBtruez]

        // where myx = mzx = mzy = 0

        // Hence:
        //  [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        //  [mBmeasy] = [by]     [0  1   0]   [0     sy  myz]    [mBtruey]
        //  [mBmeasz] = [bz]     [0  0   1]   [0     0   sz ]    [mBtruez]

        //  [mBmeasx] = [bx] +   [1+sx   mxy     mxz ][mBtruex]
        //  [mBmeasy]   [by]     [0      1+sy    myz ][mBtruey]
        //  [mBmeasz]   [bz]     [0      0       1+sz][mBtruez]

        //  mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy = by + (1+sy) * mBtruey + myz * mBtruez
        //  mBmeasz = bz + (1+sz) * mBtruez

        // Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myz
        // Reordering:
        //  mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy = by + mBtruey + sy * mBtruey + myz * mBtruez
        //  mBmeasz = bz + mBtruez + sz * mBtruez

        //  mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy - mBtruey = by + sy * mBtruey + myz * mBtruez
        //  mBmeasz - mBtruez = bz + sz * mBtruez

        // [1   0   0   mBtruex  0        0        mBtruey  mBtruez  0      ][bx ] = [mBmeasx - mBtruex]
        // [0   1   0   0        mBtruey  0        0        0        mBtruez][by ]   [mBmeasy - mBtruey]
        // [0   0   1   0        0        mBtruez  0        0        0      ][bz ]   [mBmeasz - mBtruez]
        //                                                                   [sx ]
        //                                                                   [sy ]
        //                                                                   [sz ]
        //                                                                   [mxy]
        //                                                                   [mxz]
        //                                                                   [myz]

        mFitter.setFunctionEvaluator(new LevenbergMarquardtMultiVariateFunctionEvaluator() {
            @Override
            public int getNumberOfDimensions() {
                // Input points are true magnetic flux density coordinates
                return BodyMagneticFluxDensity.COMPONENTS;
            }

            @Override
            public int getNumberOfVariables() {
                // The multivariate function returns the components of measured magnetic flux density
                return BodyMagneticFluxDensity.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final double[] initial = new double[COMMON_Z_AXIS_UNKNOWNS];

                initial[0] = mInitialHardIronX;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1805
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1879
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1181
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return mUseLinearCalibrator;
    }

    /**
     * Sepecifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(
            final boolean linearCalibratorUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mUseLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return mRefinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(
            final boolean preliminarySolutionRefined)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mRefinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(
            final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(
            final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1805
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1879
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1206
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return mUseLinearCalibrator;
    }

    /**
     * Sepecifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(
            final boolean linearCalibratorUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mUseLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return mRefinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(
            final boolean preliminarySolutionRefined)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mRefinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(
            final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(
            final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1284
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1343
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1181
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return mUseLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mUseLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return mRefinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(
            final boolean preliminarySolutionRefined) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mRefinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(
            final float progressDelta) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(
            final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(
            final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(
            final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(
            final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 918
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2375
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 925
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2936
        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containint coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mBiasX;
        result[1] = mBiasY;
        result[2] = mBiasZ;
    }

    /**
     * Sets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mBiasX = bias[0];
        mBiasY = bias[1];
        mBiasZ = bias[2];
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @return known accelerometer bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mBiasX);
        result.setElementAtIndex(1, mBiasY);
        result.setElementAtIndex(2, mBiasZ);
    }

    /**
     * Sets known accelerometer bias as a column matrix.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS
                || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mBiasX = bias.getElementAtIndex(0);
        mBiasY = bias.getElementAtIndex(1);
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Indicates whether estimator is ready to start the estimator.
     *
     * @return true if estimator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1642
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3316
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getHardIron() {
        final double[] result = new double[
                BodyMagneticFluxDensity.COMPONENTS];
        getHardIron(result);
        return result;
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mHardIronX;
        result[1] = mHardIronY;
        result[2] = mHardIronZ;
    }

    /**
     * Sets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param hardIron known hard-iron bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setHardIron(final double[] hardIron) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mHardIronX = hardIron[0];
        mHardIronY = hardIron[1];
        mHardIronZ = hardIron[2];
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @return hard-iron bias as a column matrix.
     */
    @Override
    public Matrix getHardIronMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    1);
            getHardIronMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getHardIronMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mHardIronX);
        result.setElementAtIndex(1, mHardIronY);
        result.setElementAtIndex(2, mHardIronZ);
    }

    /**
     * Sets known hard-iron bias.
     *
     * @param hardIron magnetometer hard-iron bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setHardIron(final Matrix hardIron) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS
                || hardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mHardIronX = hardIron.getElementAtIndex(0);
        mHardIronY = hardIron.getElementAtIndex(1);
        mHardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Estimates magnetometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 778
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 408
        estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, position, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(final double timeInterval,
                         final CoordinateTransformation c,
                         final CoordinateTransformation oldC,
                         final Speed vx, final Speed vy, final Speed vz,
                         final Speed oldVx, final Speed oldVy, final Speed oldVz,
                         final Point3D position, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                position, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(final Time timeInterval,
                         final CoordinateTransformation c,
                         final CoordinateTransformation oldC,
                         final Speed vx, final Speed vy, final Speed vz,
                         final Speed oldVx, final Speed oldVy, final Speed oldVz,
                         final Point3D position, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                position, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(final double timeInterval,
                                               final CoordinateTransformation c,
                                               final CoordinateTransformation oldC,
                                               final double vx, final double vy, final double vz,
                                               final double oldVx, final double oldVy, final double oldVz,
                                               final double x, final double y, final double z) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                x, y, z);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(final Time timeInterval,
                                               final CoordinateTransformation c,
                                               final CoordinateTransformation oldC,
                                               final double vx, final double vy, final double vz,
                                               final double oldVx, final double oldVy, final double oldVz,
                                               final double x, final double y, final double z) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz,
                oldVx, oldVy, oldVz, x, y, z);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(final double timeInterval,
                                               final CoordinateTransformation c,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1941
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1983
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    public int getMinimumRequiredMeasurements() {
        return mCommonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS :
                MINIMUM_MEASUREMENTS_GENERAL;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= getMinimumRequiredMeasurements()
                && mPosition != null;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 404
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator2D.java 415
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this estimator is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }


    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Solution<Point2D>> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<Solution<Point2D>>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point2D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point2D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return RANSACRobustRangingAndRssiRadioSourceEstimator2D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator3D.java 404
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 414
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this estimator is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }


    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Solution<Point3D>> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<Solution<Point3D>>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point3D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point3D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return RANSACRobustRangingAndRssiRadioSourceEstimator3D.this.isReady();
File Line
com/irurueta/navigation/indoor/position/NonLinearMixedPositionEstimator.java 116
com/irurueta/navigation/indoor/position/NonLinearRangingAndRssiPositionEstimator.java 117
com/irurueta/navigation/indoor/position/NonLinearRangingPositionEstimator.java 116
com/irurueta/navigation/indoor/position/NonLinearRssiPositionEstimator.java 115
            final P initialPosition, final MixedPositionEstimatorListener<P> listener) {
        this(listener);
        mInitialPosition = initialPosition;
    }

    /**
     * Gets initial position to start position estimation.
     * If not defined, centroid of located sources position will be used to start
     * the estimation.
     *
     * @return initial position to start position estimation.
     */
    public P getInitialPosition() {
        return mInitialPosition;
    }

    /**
     * Sets initial position to start position estimation.
     * If not defined, centroid of located sources position will be used to start
     * the estimation.
     *
     * @param initialPosition initial position to start position estimation.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialPosition(final P initialPosition) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialPosition = initialPosition;
    }

    /**
     * Indicates whether located radio source position covariance must be taken into
     * account (if available) to determine distance standard deviation.
     *
     * @return true to take radio source position covariance into account, false
     * otherwise.
     */
    public boolean isRadioSourcePositionCovarianceUsed() {
        return mUseRadioSourcePositionCovariance;
    }

    /**
     * Specifies whether located radio source position covariance must be taken into
     * account (if available) to determine distance standard deviation.
     *
     * @param useRadioSourcePositionCovariance true to take radio source position
     *                                         covariance into account, false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setRadioSourcePositionCovarianceUsed(
            final boolean useRadioSourcePositionCovariance) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mUseRadioSourcePositionCovariance = useRadioSourcePositionCovariance;
    }

    /**
     * Gets distance standard deviation fallback value to use when none can be
     * determined from provided radio sources and fingerprint readings.
     *
     * @return distance standard deviation to use as fallback.
     */
    public double getFallbackDistanceStandardDeviation() {
        return mFallbackDistanceStandardDeviation;
    }

    /**
     * Sets distance standard deviation fallback value to use when none can be
     * determined from provided radio sources and fingerprint readings.
     *
     * @param fallbackDistanceStandardDeviation distance standard deviation to use
     *                                          as fallback.
     * @throws LockedException if estimator is locked.
     */
    public void setFallbackDistanceStandardDeviation(
            final double fallbackDistanceStandardDeviation) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mFallbackDistanceStandardDeviation = fallbackDistanceStandardDeviation;
    }

    /**
     * Gets minimum required number of located radio sources to perform lateration.
     *
     * @return minimum required number of located radio sources to perform lateration.
     */
    @Override
    public int getMinRequiredSources() {
        return mTrilaterationSolver.getMinRequiredPositionsAndDistances();
    }

    /**
     * Indicates whether estimator is ready to find a solution.
     *
     * @return true if estimator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mTrilaterationSolver.isReady();
    }

    /**
     * Returns boolean indicating whether this estimator is locked because an estimation
     * is already in progress.
     *
     * @return true if estimator is locked, false otherwise.
     */
    @Override
    public boolean isLocked() {
        return mTrilaterationSolver.isLocked();
    }

    /**
     * Gets standard deviations of distances from known located radio sources to the
     * location of provided readings in a fingerprint.
     * Distance standard deviations are used internally to solve lateration.
     *
     * @return standard deviations used internally.
     */
    public double[] getDistanceStandardDeviations() {
        return mTrilaterationSolver.getDistanceStandardDeviations();
    }

    /**
     * Estimates position based on provided located radio sources and readings of such
     * radio sources at an unknown location.
     *
     * @throws LockedException             if estimator is locked.
     * @throws NotReadyException           if estimator is not ready.
     * @throws PositionEstimationException if estimation fails for some other reason.
     */
    @Override
    public void estimate() throws LockedException, NotReadyException,
            PositionEstimationException {
        try {
            mTrilaterationSolver.setInitialPosition(mInitialPosition);

            mTrilaterationSolver.solve();
            mEstimatedPositionCoordinates =
                    mTrilaterationSolver.getEstimatedPositionCoordinates();
        } catch (final LaterationException e) {
            throw new PositionEstimationException(e);
        }
    }

    /**
     * Gets known positions of radio sources used internally to solve lateration.
     *
     * @return known positions used internally.
     */
    @Override
    public P[] getPositions() {
        return mTrilaterationSolver.getPositions();
    }

    /**
     * Gets euclidean distances from known located radio sources to the location of
     * provided readings in a fingerprint.
     * Distance values are used internally to solve lateration.
     *
     * @return euclidean distances used internally.
     */
    @Override
    public double[] getDistances() {
        return mTrilaterationSolver.getDistances();
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getCovariance() {
        return mTrilaterationSolver.getCovariance();
    }

    /**
     * Internally sets located radio sources used for lateration.
     *
     * @param sources located radio sources used for lateration.
     * @throws IllegalArgumentException if provided value is null or the number of
     *                                  provided sources is less than the required minimum.
     */
    protected void internalSetSources(
            final List<? extends RadioSourceLocated<P>> sources) {
        super.internalSetSources(sources);
        buildPositionsDistancesAndDistanceStandardDeviations();
    }

    /**
     * Internally sets fingerprint containing readings at an unknown location for provided
     * located radio sources.
     *
     * @param fingerprint fingerprint containing readings at an unknown location for
     *                    provided located radio sources.
     * @throws IllegalArgumentException if provided value is null.
     */
    protected void internalSetFingerprint(
            final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint) {
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator2D.java 840
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator2D.java 334
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Solution<Point2D>> innerEstimator =
                new PROMedSRobustEstimator<>(
                        new PROMedSRobustEstimatorListener<Solution<Point2D>>() {

                            @Override
                            public double[] getQualityScores() {
                                return mQualityScores;
                            }

                            @Override
                            public double getThreshold() {
                                return mStopThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point2D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point2D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return PROMedSRobustRangingAndRssiRadioSourceEstimator2D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator3D.java 838
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator3D.java 334
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator3D.java 852
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Solution<Point3D>> innerEstimator =
                new PROMedSRobustEstimator<>(
                        new PROMedSRobustEstimatorListener<Solution<Point3D>>() {

                            @Override
                            public double[] getQualityScores() {
                                return mQualityScores;
                            }

                            @Override
                            public double getThreshold() {
                                return mStopThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point3D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point3D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return PROMedSRobustRangingAndRssiRadioSourceEstimator3D.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 5732
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1018
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1115
                bias, initialMa, listener);
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return x-coordinate of known accelerometer bias.
     */
    public double getBiasX() {
        return mBiasX;
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final double biasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return y-coordinate of known accelerometer bias.
     */
    public double getBiasY() {
        return mBiasY;
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final double biasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = biasY;
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return z-coordinate of known accelerometer bias.
     */
    public double getBiasZ() {
        return mBiasZ;
    }

    /**
     * Sets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final double biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = biasZ;
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @return x-coordinate of known accelerometer bias.
     */
    public Acceleration getBiasXAsAcceleration() {
        return new Acceleration(mBiasX,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasXAsAcceleration(final Acceleration result) {
        result.setValue(mBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final Acceleration biasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = convertAcceleration(biasX);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @return y-coordinate of known accelerometer bias.
     */
    public Acceleration getBiasYAsAcceleration() {
        return new Acceleration(mBiasY,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasYAsAcceleration(final Acceleration result) {
        result.setValue(mBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final Acceleration biasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = convertAcceleration(biasY);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @return z-coordinate of known accelerometer bias.
     */
    public Acceleration getBiasZAsAcceleration() {
        return new Acceleration(mBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasZAsAcceleration(final Acceleration result) {
        result.setValue(mBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets z-coordinate of known accelerometer bias to be used to find a solution.
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final Acceleration biasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Sets known bias coordinates of accelerometer expressed in meters
     * per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @param biasY y-coordinate of known accelerometer bias.
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(final double biasX, final double biasY,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1866
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3098
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1047
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1872
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3058
        mAccelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return x-coordinate of gyroscope known bias.
     */
    public double getBiasX() {
        return mBiasX;
    }

    /**
     * Sets x-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final double biasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return y-coordinate of gyroscope known bias.
     */
    public double getBiasY() {
        return mBiasY;
    }

    /**
     * Sets y-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasY y-coordinate of gyroscope known  bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final double biasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = biasY;
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return z-coordinate of gyroscope known bias.
     */
    public double getBiasZ() {
        return mBiasZ;
    }

    /**
     * Sets z-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasZ z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final double biasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = biasZ;
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     *
     * @return x-coordinate of gyroscope known bias.
     */
    public AngularSpeed getBiasAngularSpeedX() {
        return new AngularSpeed(mBiasX,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(mBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets x-coordinate of gyroscope known bias.
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final AngularSpeed biasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = convertAngularSpeed(biasX);
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     *
     * @return y-coordinate of gyroscope known bias.
     */
    public AngularSpeed getBiasAngularSpeedY() {
        return new AngularSpeed(mBiasY,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(mBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets y-coordinate of gyroscope known bias.
     *
     * @param biasY y-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final AngularSpeed biasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = convertAngularSpeed(biasY);
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     *
     * @return initial z-coordinate of gyroscope known bias.
     */
    public AngularSpeed getBiasAngularSpeedZ() {
        return new AngularSpeed(mBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    public void getBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(mBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets z-coordinate of gyroscope known bias.
     *
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final AngularSpeed biasZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Sets known bias coordinates of gyroscope expressed in
     * radians per second (rad/s).
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @param biasY y-coordinate of gyroscope known bias.
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 997
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2089
        fillHardIronBiases(bx, by, bz);
        fillMm(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException if there are numerical errors.
     * @throws IOException      if world magnetic model cannot be loaded.
     */
    private void calibrateGeneral() throws AlgebraException, IOException {
        // The magnetometer model is:
        // mBmeas = bm + (I + Mm) * mBtrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // mBmeas = bm + (I + Mm) * mBtrue

        // Hence:
        //  [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        //  [mBmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [mBtruey]
        //  [mBmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [mBtruez]

        //  [mBmeasx] = [bx] +   [1+sx   mxy     mxz ][mBtruex]
        //  [mBmeasy]   [by]     [myx    1+sy    myz ][mBtruey]
        //  [mBmeasz]   [bz]     [mzx    mzy     1+sz][mBtruez]

        //  mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy = by + myx * mBtruex + (1+sy) * mBtruey + myz * mBtruez
        //  mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + (1+sz) * mBtruez

        // Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
        // Reordering:
        //  mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy = by + myx * mBtruex + mBtruey + sy * mBtruey + myz * mBtruez
        //  mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + mBtruez + sz * mBtruez

        //  mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy - mBtruey = by + myx * mBtruex + sy * mBtruey + myz * mBtruez
        //  mBmeasz - mBtruez = bz + mzx * mBtruex + mzy * mBtruey + sz * mBtruez

        // [1   0   0   mBtruex  0        0        mBtruey  mBtruez  0        0        0        0      ][bx ] = [mBmeasx - mBtruex]
        // [0   1   0   0        mBtruey  0        0        0        mBtruex  mBtruez  0        0      ][by ]   [mBmeasy - mBtruey]
        // [0   0   1   0        0        mBtruez  0        0        0        0        mBtruex  mBtruey][bz ]   [mBmeasz - mBtruez]
        //                                                                                              [sx ]
        //                                                                                              [sy ]
        //                                                                                              [sz ]
        //                                                                                              [mxy]
        //                                                                                              [mxz]
        //                                                                                              [myx]
        //                                                                                              [myz]
        //                                                                                              [mzx]
        //                                                                                              [mzy]

        final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
        if (mMagneticModel != null) {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(mMagneticModel);
        } else {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
        }

        final BodyMagneticFluxDensity expectedMagneticFluxDensity =
                new BodyMagneticFluxDensity();
        final NEDFrame nedFrame = new NEDFrame();
        final NEDMagneticFluxDensity earthB = new NEDMagneticFluxDensity();
        final CoordinateTransformation cbn = new CoordinateTransformation(
                FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final CoordinateTransformation cnb = new CoordinateTransformation(
                FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);

        final int rows = EQUATIONS_PER_MEASUREMENT * mMeasurements.size();
        final Matrix a = new Matrix(rows, GENERAL_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyMagneticFluxDensity measurement : mMeasurements) {
            final BodyMagneticFluxDensity measuredMagneticFluxDensity =
                    measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final ECEFFrame ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final double year = measurement.getYear();

            final double latitude = nedFrame.getLatitude();
            final double longitude = nedFrame.getLongitude();
            final double height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body mangetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb,
                    expectedMagneticFluxDensity);

            final double bMeasX = measuredMagneticFluxDensity.getBx();
            final double bMeasY = measuredMagneticFluxDensity.getBy();
            final double bMeasZ = measuredMagneticFluxDensity.getBz();

            final double bTrueX = expectedMagneticFluxDensity.getBx();
            final double bTrueY = expectedMagneticFluxDensity.getBy();
            final double bTrueZ = expectedMagneticFluxDensity.getBz();

            a.setElementAt(i, 0, 1.0);
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator2D.java 838
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator2D.java 850
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Solution<Point2D>> innerEstimator =
                new PROMedSRobustEstimator<>(
                        new PROMedSRobustEstimatorListener<Solution<Point2D>>() {

                            @Override
                            public double[] getQualityScores() {
                                return mQualityScores;
                            }

                            @Override
                            public double getThreshold() {
                                return mStopThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point2D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point2D> currentEstimation,
File Line
com/irurueta/navigation/indoor/Utils.java 260
com/irurueta/navigation/indoor/Utils.java 799
com/irurueta/navigation/indoor/Utils.java 1965
    public static MultivariateNormalDist propagateVariancesToRssiVarianceFirstOrderNonLinear2D(
            final double fingerprintRssi, final double pathLossExponent,
            final Point2D fingerprintPosition, final Point2D radioSourcePosition,
            final Point2D estimatedPosition,
            final Double fingerprintRssiVariance,
            final Double pathLossExponentVariance,
            final Matrix fingerprintPositionCovariance,
            final Matrix radioSourcePositionCovariance,
            final Matrix estimatedPositionCovariance) throws IndoorException {

        if (fingerprintPosition == null || radioSourcePosition == null ||
                estimatedPosition == null) {
            return null;
        }

        //1st order Taylor expression of received power in 2D:
        //Pr(pi) = Pr(p1)
        //  - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
        //  - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
        //where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2

        final double x1 = fingerprintPosition.getInhomX();
        final double y1 = fingerprintPosition.getInhomY();

        final double xa = radioSourcePosition.getInhomX();
        final double ya = radioSourcePosition.getInhomY();

        final double xi = estimatedPosition.getInhomX();
        final double yi = estimatedPosition.getInhomY();

        final double[] mean = new double[]{
                fingerprintRssi, pathLossExponent, x1, y1, xa, ya, xi, yi
        };
        final Matrix covariance = Matrix.diagonal(new double[]{
                fingerprintRssiVariance != null ? fingerprintRssiVariance : 0.0,
                pathLossExponentVariance != null ? pathLossExponentVariance : 0.0,
                0.0, 0.0, 0.0, 0.0, 0.0, 0.0
        });

        if (fingerprintPositionCovariance != null &&
                fingerprintPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH &&
                fingerprintPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {

            covariance.setSubmatrix(2, 2,
                    3, 3,
                    fingerprintPositionCovariance);
        }

        if (radioSourcePositionCovariance != null &&
                radioSourcePositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH &&
                radioSourcePositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
            covariance.setSubmatrix(4, 4,
                    5, 5,
                    radioSourcePositionCovariance);
        }

        if (estimatedPositionCovariance != null &&
                estimatedPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH &&
                estimatedPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
            covariance.setSubmatrix(6, 6,
                    7, 7,
                    estimatedPositionCovariance);
        }

        try {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3230
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4714
                        return evaluateCommonAxisWithGDependentCrossBiases(mI, params);
                    }
                });

        final Matrix initialM = Matrix.identity(
                BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        initialM.add(getInitialMg());

        // Force initial M to be upper diagonal
        initialM.setElementAt(1, 0, 0.0);
        initialM.setElementAt(2, 0, 0.0);
        initialM.setElementAt(2, 1, 0.0);

        final Matrix invInitialM = Utils.inverse(initialM);
        final Matrix initialBg = getInitialBiasAsMatrix();
        final Matrix initialB = invInitialM.multiplyAndReturnNew(initialBg);
        final Matrix initialGg = getInitialGg();
        final Matrix initialG = invInitialM.multiplyAndReturnNew(initialGg);

        mFitter.setFunctionEvaluator(
                new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
                    @Override
                    public int getNumberOfDimensions() {
                        // Before and after normalized gravity versors
                        return 2 * BodyKinematics.COMPONENTS;
                    }

                    @Override
                    public double[] createInitialParametersArray() {
                        final double[] initial =
                                new double[COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES];

                        // biases b
                        for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                            initial[i] = initialB.getElementAtIndex(i);
                        }

                        // upper diagonal cross coupling errors M
                        int k = BodyKinematics.COMPONENTS;
                        for (int j = 0; j < BodyKinematics.COMPONENTS; j++) {
                            for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        // g-dependent cross biases G
                        final int num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                        for (int i = 0, j = k; i < num; i++, j++) {
                            initial[j] = initialG.getElementAtIndex(i);
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1529
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 1097
    }

    /**
     * Fills scale factor and cross coupling error matrix with estimated values.
     *
     * @param sx  x scale factor
     * @param sy  y scale factor
     * @param sz  z scale factor
     * @param mxy x-y cross coupling
     * @param mxz x-z cross coupling
     * @param myx y-x cross coupling
     * @param myz y-z cross coupling
     * @param mzx z-x cross coupling
     * @param mzy z-y cross coupling
     * @throws WrongSizeException never happens.
     */
    private void fillMg(final double sx, final double sy, final double sz,
                        final double mxy, final double mxz, final double myx,
                        final double myz, final double mzx, final double mzy)
            throws WrongSizeException {
        if (mEstimatedMg == null) {
            mEstimatedMg = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
        }

        mEstimatedMg.setElementAt(0, 0, sx);
        mEstimatedMg.setElementAt(1, 0, myx);
        mEstimatedMg.setElementAt(2, 0, mzx);

        mEstimatedMg.setElementAt(0, 1, mxy);
        mEstimatedMg.setElementAt(1, 1, sy);
        mEstimatedMg.setElementAt(2, 1, mzy);

        mEstimatedMg.setElementAt(0, 2, mxz);
        mEstimatedMg.setElementAt(1, 2, myz);
        mEstimatedMg.setElementAt(2, 2, sz);
    }

    /**
     * Fills G-dependant cross biases.
     *
     * @param g11 element 1,1 of G-dependant cross biases matrix.
     * @param g12 element 1,2 of G-dependant cross biases matrix.
     * @param g13 element 1,3 of G-dependant cross biases matrix.
     * @param g21 element 2,1 of G-dependant cross biases matrix.
     * @param g22 element 2,2 of G-dependant cross biases matrix.
     * @param g23 element 2,3 of G-dependant cross biases matrix.
     * @param g31 element 3,1 of G-dependant cross biases matrix.
     * @param g32 element 3,2 of G-dependant cross biases matrix.
     * @param g33 element 3,3 of G-dependant cross biases matrix.
     * @throws WrongSizeException never happens.
     */
    private void fillGg(final double g11, final double g12, final double g13,
                        final double g21, final double g22, final double g23,
                        final double g31, final double g32, final double g33)
            throws WrongSizeException {
        if (mEstimatedGg == null) {
            mEstimatedGg = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
        }

        mEstimatedGg.setElementAt(0, 0, g11);
        mEstimatedGg.setElementAt(0, 1, g12);
        mEstimatedGg.setElementAt(0, 2, g13);

        mEstimatedGg.setElementAt(1, 0, g21);
        mEstimatedGg.setElementAt(1, 1, g22);
        mEstimatedGg.setElementAt(1, 2, g23);

        mEstimatedGg.setElementAt(2, 0, g31);
        mEstimatedGg.setElementAt(2, 1, g32);
        mEstimatedGg.setElementAt(2, 2, g33);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 352
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 358
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 358
            final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mStopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustKnownFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1351
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1319
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mSequences.size();
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new PROMedSRobustEstimator<>(
                        new PROMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mStopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation,
                            final int i) {
                        return computeError(mSequences.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustEasyGyroscopeCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3846
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3909
                commonAxisUsed, estimateGDependentCrossBiases, bias,
                initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new PROMedSRobustEstimator<>(
                        new PROMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mStopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1683
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1701
        super(position, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        mGravityNorm = computeGravityNorm();

        final PROMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mStopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2348
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1306
                                          final ECEFFrame frame,
                                          final CoordinateTransformation oldC,
                                          final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                          final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC,
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final double timeInterval,
                                          final CoordinateTransformation c,
                                          final CoordinateTransformation oldC,
                                          final double vx, final double vy, final double vz,
                                          final double oldVx, final double oldVy, final double oldVz,
                                          final Point3D position, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final Time timeInterval,
                                          final CoordinateTransformation c,
                                          final CoordinateTransformation oldC,
                                          final double vx, final double vy, final double vz,
                                          final double oldVx, final double oldVy, final double oldVz,
                                          final Point3D position, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final double timeInterval,
                                          final CoordinateTransformation c,
                                          final CoordinateTransformation oldC,
                                          final ECEFVelocity velocity,
File Line
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 406
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator2D.java 175
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator2D.java 417
                initialPathLossExponent, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this estimator is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }


    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Solution<Point2D>> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<Solution<Point2D>>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point2D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point2D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return RANSACRobustRangingAndRssiRadioSourceEstimator2D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator3D.java 406
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator3D.java 175
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 416
                initialPathLossExponent, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this estimator is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }


    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Solution<Point3D>> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<Solution<Point3D>>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point3D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point3D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return RANSACRobustRangingAndRssiRadioSourceEstimator3D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator.java 477
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator.java 1639
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator.java 1619
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPowerdBm() {
        return mInitialTransmittedPowerdBm;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     *
     * @param initialTransmittedPowerdBm initial transmitted power to start the
     *                                   estimation of radio source transmitted
     *                                   power.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialTransmittedPowerdBm(final Double initialTransmittedPowerdBm)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialTransmittedPowerdBm = initialTransmittedPowerdBm;
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPower() {
        return mInitialTransmittedPowerdBm != null ?
                Utils.dBmToPower(mInitialTransmittedPowerdBm) : null;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     *
     * @param initialTransmittedPower initial transmitted power to start the
     *                                estimation of radio source transmitted power.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setInitialTransmittedPower(final Double initialTransmittedPower)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (initialTransmittedPower != null) {
            if (initialTransmittedPower < 0.0) {
                throw new IllegalArgumentException();
            }
            mInitialTransmittedPowerdBm = Utils.powerTodBm(
                    initialTransmittedPower);
        } else {
            mInitialTransmittedPowerdBm = null;
        }
    }

    /**
     * Gets initial position to start the estimation of radio source position.
     * If not defined, centroid of provided fingerprints will be used.
     *
     * @return initial position to start the estimation of radio source position.
     */
    public P getInitialPosition() {
        return mInitialPosition;
    }

    /**
     * Sets initial position to start the estimation of radio source position.
     * If not defined, centroid of provided fingerprints will be used.
     *
     * @param initialPosition initial position to start the estimation of radio
     *                        source position.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialPosition(final P initialPosition) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialPosition = initialPosition;
    }

    /**
     * Gets initial exponent typically used on free space for path loss propagation
     * in terms of distance.
     * On different environments path loss exponent might have different value:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     * <p>
     * If path loss exponent estimation is enabled, estimation will start at this
     * value and will converge to the most appropriate value.
     * If path loss exponent estimation is disabled, this value will be assumed
     * to be exact and the estimated path loss exponent will be equal to this
     * value.
     *
     * @return initial path loss exponent.
     */
    public double getInitialPathLossExponent() {
        return mInitialPathLossExponent;
    }

    /**
     * Sets initial exponent typically used on free space for path loss propagation
     * in terms of distance.
     * On different environments path loss exponent might have different value:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     * <p>
     * If path loss exponent estimation is enabled, estimation will start at this
     * value and will converge to the most appropriate value.
     * If path loss exponent estimation is disabled, this value will be assumed
     * to be exact and the estimated path loss exponent will be equal to this
     * value.
     *
     * @param initialPathLossExponent initial path loss exponent.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialPathLossExponent(final double initialPathLossExponent)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialPathLossExponent = initialPathLossExponent;
    }

    /**
     * Indicates whether transmitted power estimation is enabled or not.
     *
     * @return true if transmitted power estimation is enabled, false otherwise.
     */
    public boolean isTransmittedPowerEstimationEnabled() {
        return mTransmittedPowerEstimationEnabled;
    }

    /**
     * Specifies whether transmitted power estimation is enabled or not.
     *
     * @param transmittedPowerEstimationEnabled true if transmitted power estimation is enabled,
     *                                          false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setTransmittedPowerEstimationEnabled(
            final boolean transmittedPowerEstimationEnabled)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mTransmittedPowerEstimationEnabled = transmittedPowerEstimationEnabled;
    }

    /**
     * Indicates whether path loss estimation is enabled or not.
     *
     * @return true if path loss estimation is enabled, false otherwise.
     */
    public boolean isPathLossEstimationEnabled() {
        return mPathLossEstimationEnabled;
    }

    /**
     * Specifies whether path loss estimation is enabled or not.
     *
     * @param pathLossEstimationEnabled true if path loss estimation is enabled,
     *                                  false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setPathLossEstimationEnabled(
            final boolean pathLossEstimationEnabled)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mPathLossEstimationEnabled = pathLossEstimationEnabled;
    }

    /**
     * Indicates whether position covariances of readings must be taken into account to increase
     * the amount of standard deviation of each ranging measure by the amount of position standard
     * deviation assuming that both measures are statistically independent.
     *
     * @return true to take into account reading position covariances, false otherwise.
     */
    public boolean getUseReadingPositionCovariance() {
        return mUseReadingPositionCovariances;
    }

    /**
     * Specifies whether position covariances of readings must be taken into account to increase
     * the amount of standard deviation of each ranging measure by the amount of position standard
     * deviation assuming that both measures are statistically independent.
     *
     * @param useReadingPositionCovariances true to take into account reading position covariances, false
     *                                      otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setUseReadingPositionCovariances(
            final boolean useReadingPositionCovariances)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mUseReadingPositionCovariances = useReadingPositionCovariances;
    }

    /**
     * Indicates whether an homogeneous linear solver is used to estimate an initial
     * position for the internal ranging radio source estimator.
     *
     * @return true if homogeneous linear solver is used, false if an inhomogeneous linear
     * one is used instead.
     */
    public abstract boolean isHomogeneousRangingLinearSolverUsed();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1577
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 359
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Matrix> innerEstimator =
                new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mStopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<Matrix> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final Matrix currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 353
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1585
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 359
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 359
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mStopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustKnownFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4127
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4183
                return KnownBiasTurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_GENERAL;
            }
        }
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    public boolean isReady() {
        return mMeasurements != null
                && mMeasurements.size() >= getMinimumRequiredMeasurements();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public abstract void calibrate() throws LockedException, NotReadyException,
            CalibrationException;

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7790
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5727
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2787
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters for the general case.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the general purpose case.
     *               Must have length 12.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneral(final double[] params) throws EvaluationException {
        final double bx = params[0];
        final double by = params[1];
        final double bz = params[2];

        final double m11 = params[3];
        final double m21 = params[4];
        final double m31 = params[5];

        final double m12 = params[6];
        final double m22 = params[7];
        final double m32 = params[8];

        final double m13 = params[9];
        final double m23 = params[10];
        final double m33 = params[11];

        return evaluate(bx, by, bz, m11, m21, m31, m12, m22, m32,
                m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters when common z-axis is assumed.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the common z-axis case.
     *               Must have length 9.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateCommonAxis(final double[] params) throws EvaluationException {
        final double bx = params[0];
        final double by = params[1];
        final double bz = params[2];

        final double m11 = params[3];

        final double m12 = params[4];
        final double m22 = params[5];

        final double m13 = params[6];
        final double m23 = params[7];
        final double m33 = params[8];

        return evaluate(bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0,
                m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param bx  x-coordinate of bias.
     * @param by  y-coordinate of bias.
     * @param bz  z-coordinate of bias.
     * @param m11 element 1,1 of cross-coupling error matrix.
     * @param m21 element 2,1 of cross-coupling error matrix.
     * @param m31 element 3,1 of cross-coupling error matrix.
     * @param m12 element 1,2 of cross-coupling error matrix.
     * @param m22 element 2,2 of cross-coupling error matrix.
     * @param m32 element 3,2 of cross-coupling error matrix.
     * @param m13 element 1,3 of cross-coupling error matrix.
     * @param m23 element 2,3 of cross-coupling error matrix.
     * @param m33 element 3,3 of cross-coupling error matrix.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluate(final double bx, final double by, final double bz,
                            final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33)
File Line
com/irurueta/navigation/indoor/RadioSourceKNearestFinder.java 43
com/irurueta/navigation/indoor/RadioSourceNoMeanKNearestFinder.java 47
    public RadioSourceKNearestFinder(
            final Collection<? extends RssiFingerprintLocated<S, RssiReading<S>, P>> fingerprints) {
        if (fingerprints == null) {
            throw new IllegalArgumentException();
        }
        mFingerprints = fingerprints;
    }

    /**
     * Finds nearest fingerprint to provided one, in terms of signal euclidean distances, within the collection of
     * provided fingerprints.
     *
     * @param fingerprint fingerprint to find the nearest to.
     * @return nearest fingerprint or null if none could be found.
     */
    public RssiFingerprintLocated<S, RssiReading<S>, P> findNearestTo(
            final RssiFingerprint<S, RssiReading<S>> fingerprint) {
        return findNearestTo(fingerprint, mFingerprints);
    }

    /**
     * Finds k-nearest fingerprints to provided one, in terms of signal euclidean distances, within the collection
     * of provided fingerprints.
     *
     * @param fingerprint fingerprint to find the k-nearest ones to.
     * @param k           number of nearest fingerprints to find.
     * @return nearest fingerprints ordered from closest to farthest or an empty list if none could be found.
     * @throws IllegalArgumentException if either fingerprint is null or k is les than 1.
     */
    public List<RssiFingerprintLocated<S, RssiReading<S>, P>> findKNearestTo(
            final RssiFingerprint<S, RssiReading<S>> fingerprint,
            final int k) {
        return findKNearestTo(fingerprint, mFingerprints, k);
    }

    /**
     * Finds k-nearest fingerprints to provided one, in terms of signal euclidean distances, within the collection
     * of provided fingerprints.
     *
     * @param fingerprint         fingerprint to find the k-nearest ones to.
     * @param k                   number of nearest fingerprints to find.
     * @param nearestFingerprints list where found nearest fingerprints will be stored ordered from closest to farthest
     *                            or an empty list if none could be found.
     * @param nearestSqrDistances list where squared signal euclidean distances corresponding to found fingerprints will
     *                            be stored or an empty list if no fingerprint is found.
     * @throws IllegalArgumentException if any parameter is null or k is less than 1.
     */
    public void findKNearestTo(
            final RssiFingerprint<S, RssiReading<S>> fingerprint,
            final int k,
            final List<RssiFingerprintLocated<S, RssiReading<S>, P>> nearestFingerprints,
            final List<Double> nearestSqrDistances) {
        findKNearestTo(fingerprint, mFingerprints, k, nearestFingerprints, nearestSqrDistances);
    }

    /**
     * Gets collection of fingerprints to match against.
     *
     * @return collection of fingerprints to match against.
     */
    public Collection<? extends RssiFingerprintLocated<S, RssiReading<S>, P>> getFingerprints() {
        return mFingerprints;
    }

    /**
     * Finds nearest fingerprint to provided one, in terms of signal euclidean distances, within the collection of
     * provided fingerprints.
     *
     * @param fingerprint  fingerprint to find the nearest to.
     * @param fingerprints collection of fingerprints to make the search for the nearest one.
     * @param <P>          a {@link Point} type.
     * @param <S>          a {@link RadioSource} type.
     * @return nearest fingerprint or null if none could be found.
     * @throws IllegalArgumentException if either fingerprint or collection of fingerprints is null.
     */
    @SuppressWarnings("Duplicates")
    public static <P extends Point<?>, S extends RadioSource> RssiFingerprintLocated<S, RssiReading<S>, P> findNearestTo(
            final RssiFingerprint<S, RssiReading<S>> fingerprint,
            final Collection<? extends RssiFingerprintLocated<S, RssiReading<S>, P>> fingerprints) {
        if (fingerprint == null || fingerprints == null) {
            throw new IllegalArgumentException();
        }

        double bestSqrDist = Double.MAX_VALUE;
        RssiFingerprintLocated<S, RssiReading<S>, P> result = null;
        for (final RssiFingerprintLocated<S, RssiReading<S>, P> f : fingerprints) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1577
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 359
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2065
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Matrix> innerEstimator =
                new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mStopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<Matrix> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final Matrix currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 353
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1585
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3847
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 359
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3910
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 359
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2062
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mStopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustKnownFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator2D.java 334
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator2D.java 852
        super(readings, initialPosition, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Robustly estimates position for a radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Solution<Point2D>> innerEstimator =
                new PROMedSRobustEstimator<>(
                        new PROMedSRobustEstimatorListener<Solution<Point2D>>() {

                            @Override
                            public double[] getQualityScores() {
                                return mQualityScores;
                            }

                            @Override
                            public double getThreshold() {
                                return mStopThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point2D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point2D> currentEstimation,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 6086
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5880
            mAngularRateFixer.setBias(preliminaryResult.mEstimatedBiases);
            mAngularRateFixer.setCrossCouplingErrors(
                    preliminaryResult.mEstimatedMg);
            mAngularRateFixer.setGDependantCrossBias(
                    preliminaryResult.mEstimatedGg);

            // copy measured sequence as it will be used to fix kinematics values
            // using preliminary gyroscope parameters
            final BodyKinematicsSequence<StandardDeviationTimedBodyKinematics> fixedSequence =
                    new BodyKinematicsSequence<>(sequence);

            // fix body kinematic measurements of provided sequence
            final int numItems = sequence.getItemsCount();
            final List<StandardDeviationTimedBodyKinematics> measuredItems = sequence.getSortedItems();
            final List<StandardDeviationTimedBodyKinematics> fixedItems = fixedSequence.getSortedItems();
            for (int j = 0; j < numItems; j++) {
                final StandardDeviationTimedBodyKinematics measuredItem = measuredItems.get(j);
                final StandardDeviationTimedBodyKinematics fixedItem = fixedItems.get(j);

                final BodyKinematics measuredKinematics = measuredItem
                        .getKinematics();
                final BodyKinematics fixedKinematics = fixedItem
                        .getKinematics();
                fixKinematics(measuredKinematics, fixedKinematics);
            }

            // integrate fixed sequence to obtain attitude change
            QuaternionIntegrator.integrateGyroSequence(fixedSequence, mQ);

            // fix before coordinates
            mMeasuredSpecificForce[0] = sequence.getBeforeMeanFx();
            mMeasuredSpecificForce[1] = sequence.getBeforeMeanFy();
            mMeasuredSpecificForce[2] = sequence.getBeforeMeanFz();
            mAccelerationFixer.fix(mMeasuredSpecificForce,
                    mFixedSpecificForce);

            // normalize coordinates
            ArrayUtils.normalize(mFixedSpecificForce);

            // compute estimated normalized end coordinates
            mStartPoint.setCoordinates(mFixedSpecificForce);
            mQ.inverse();
            mQ.rotate(mStartPoint, mEndPoint);

            // fix after coordinates
            mMeasuredSpecificForce[0] = sequence.getAfterMeanFx();
            mMeasuredSpecificForce[1] = sequence.getAfterMeanFy();
            mMeasuredSpecificForce[2] = sequence.getAfterMeanFz();
            mAccelerationFixer.fix(mMeasuredSpecificForce,
                    mFixedSpecificForce);

            // normalize coordinates
            ArrayUtils.normalize(mFixedSpecificForce);

            mExpectedEndPoint.setCoordinates(mFixedSpecificForce);

            // compare estimated normalized end coordinates with expected
            // ones
            return mExpectedEndPoint.distanceTo(mEndPoint);

        } catch (final AlgebraException e) {
            return Double.MAX_VALUE;
        }
    }

    /**
     * Computes a preliminary solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminary solution will be stored.
     */
    protected void computePreliminarySolutions(final int[] samplesIndices,
                                               final List<PreliminaryResult> solutions) {
        final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences =
                new ArrayList<>();

        for (final int samplesIndex : samplesIndices) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2119
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1750
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration solution.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
            throw new IllegalArgumentException();
        }

        mPreliminarySubsetSize = preliminarySubsetSize;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public abstract void calibrate() throws LockedException, NotReadyException,
            CalibrationException;

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    public abstract RobustEstimatorMethod getMethod();

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param method robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
File Line
com/irurueta/navigation/lateration/HomogeneousLinearLeastSquaresLateration3DSolver.java 93
com/irurueta/navigation/lateration/InhomogeneousLinearLeastSquaresLateration3DSolver.java 93
    public HomogeneousLinearLeastSquaresLateration3DSolver(
            final Sphere[] spheres,
            final LaterationSolverListener<Point3D> listener) {
        super(listener);
        internalSetSpheres(spheres);
    }

    /**
     * Gets spheres defined by provided positions and distances.
     *
     * @return spheres defined by provided positions and distances.
     */
    public Sphere[] getSpheres() {
        if (mPositions == null) {
            return null;
        }

        final Sphere[] result = new Sphere[mPositions.length];

        for (int i = 0; i < mPositions.length; i++) {
            result[i] = new Sphere(mPositions[i], mDistances[i]);
        }
        return result;
    }

    /**
     * Sets spheres defining positions and euclidean distances.
     *
     * @param spheres spheres defining positions and distances.
     * @throws IllegalArgumentException if spheres is null or length of array of spheres
     *                                  is less than 2.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setSpheres(final Sphere[] spheres) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSpheres(spheres);
    }

    /**
     * Gets number of dimensions of provided points.
     *
     * @return always returns 2 dimensions.
     */
    @Override
    public int getNumberOfDimensions() {
        return Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH;
    }

    /**
     * Minimum required number of positions and distances.
     * At least 4 positions and distances will be required to linearly solve a 3D problem.
     *
     * @return minimum required number of positions and distances.
     */
    @Override
    public int getMinRequiredPositionsAndDistances() {
        return Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
    }

    /**
     * Gets estimated position.
     *
     * @return estimated position.
     */
    @Override
    public Point3D getEstimatedPosition() {
        if (mEstimatedPositionCoordinates == null) {
            return null;
        }

        final InhomogeneousPoint3D position = new InhomogeneousPoint3D();
        getEstimatedPosition(position);
        return position;
    }

    /**
     * Internally sets spheres defining positions and euclidean distances.
     *
     * @param spheres spheres defining positions and distances.
     * @throws IllegalArgumentException if spheres is null or length of array of spheres
     *                                  is less than 4.
     */
    private void internalSetSpheres(final Sphere[] spheres) {
        if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final Point3D[] positions = new Point3D[spheres.length];
        final double[] distances = new double[spheres.length];
        for (int i = 0; i < spheres.length; i++) {
            final Sphere sphere = spheres[i];
            positions[i] = sphere.getCenter();
            distances[i] = sphere.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 490
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3142
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a solutions.
     * This is expressed in radians per second (rad/s) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(mInitialBiasX,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(mInitialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final AngularSpeed initialBiasX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(mInitialBiasY,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(mInitialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final AngularSpeed initialBiasY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(mInitialBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(mInitialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final AngularSpeed initialBiasZ)
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2927
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3186
            final KnownFrameAccelerometerNonLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException |
                com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 189
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 195
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 191
            final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<PreliminaryResult>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mMeasurements.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return mPreliminarySubsetSize;
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<PreliminaryResult> solutions) {
                                computePreliminarySolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final PreliminaryResult currentEstimation, final int i) {
                                return computeError(mMeasurements.get(i), currentEstimation);
                            }

                            @Override
                            public boolean isReady() {
                                return RANSACRobustKnownFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 660
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 645
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices,
                                solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation,
                            final int i) {
                        return computeError(mSequences.get(i),
                                currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return RANSACRobustEasyGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1855
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1885
                commonAxisUsed, estimateGDependentCrossBiases, bias,
                initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<PreliminaryResult>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mMeasurements.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return mPreliminarySubsetSize;
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<PreliminaryResult> solutions) {
                                computePreliminarySolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final PreliminaryResult currentEstimation, final int i) {
                                return computeError(mMeasurements.get(i), currentEstimation);
                            }

                            @Override
                            public boolean isReady() {
                                return RANSACRobustKnownBiasTurntableGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 809
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 819
        super(position, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        mGravityNorm = computeGravityNorm();

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<PreliminaryResult>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mMeasurements.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return mPreliminarySubsetSize;
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<PreliminaryResult> solutions) {
                                computePreliminarySolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final PreliminaryResult currentEstimation,
                                    final int i) {
                                return computeError(mMeasurements.get(i), currentEstimation);
                            }

                            @Override
                            public boolean isReady() {
                                return RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3548
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3918
        if (mEstimatedMg == null) {
            mEstimatedMg = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
        } else {
            mEstimatedMg.initialize(0.0);
        }

        mEstimatedMg.setElementAt(0, 0, sx);

        mEstimatedMg.setElementAt(0, 1, mxy);
        mEstimatedMg.setElementAt(1, 1, sy);

        mEstimatedMg.setElementAt(0, 2, mxz);
        mEstimatedMg.setElementAt(1, 2, myz);
        mEstimatedMg.setElementAt(2, 2, sz);

        if (mEstimatedGg == null) {
            mEstimatedGg = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
        } else {
            mEstimatedGg.initialize(0.0);
        }

        mEstimatedGg.setElementAtIndex(0, g11);
        mEstimatedGg.setElementAtIndex(1, g21);
        mEstimatedGg.setElementAtIndex(2, g31);
        mEstimatedGg.setElementAtIndex(3, g12);
        mEstimatedGg.setElementAtIndex(4, g22);
        mEstimatedGg.setElementAtIndex(5, g32);
        mEstimatedGg.setElementAtIndex(6, g13);
        mEstimatedGg.setElementAtIndex(7, g23);
        mEstimatedGg.setElementAtIndex(8, g33);

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {

        // The gyroscope model is:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Hence:
        // [Ωmeasx] = [bx] + ( [1   0   0] + [sx    mxy    mxz]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0   1   0]   [myx   sy     myz]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0   0   1]   [mzx   mzy    sz ]  [Ωtruez]   [g31   g32   g33][ftruez]

        // [Ωmeasx] = [bx] + ( [1+sx  mxy    mxz ]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [myx   1+sy   myz ]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [mzx   mzy    1+sz]  [Ωtruez]   [g31   g32   g33][ftruez]

        // Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + myx * Ωtruex + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy, g11, g12, g13, g21, g22, g23,
        // g31, g32, g33
        // Reordering:
        // Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + myx * Ωtruex + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Ωmeasx - Ωtruex - bx = sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy - Ωtruey - by = myx * Ωtruex + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz - Ωtruez - bz = mzx * Ωtruex + mzy * Ωtruey + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // [Ωtruex  0       0       Ωtruey  Ωtruez  0       0       0       0       ftruex  ftruey  ftruez  0       0       0       0       0       0     ][sx ] =  [Ωmeasx - Ωtruex - bx]
        // [0       Ωtruey  0       0       0       Ωtruex  Ωtruez  0       0       0       0       0       ftruex  ftruey  ftruez  0       0       0     ][sy ]    [Ωmeasy - Ωtruey - by]
        // [0       0       Ωtruez  0       0       0       0       Ωtruex  Ωtruey  0       0       0       0       0       0       ftruex  ftruey  ftruez][sz ]    [Ωmeasz - Ωtruez - bz]
        //                                                                                                                                                 [mxy]
        //                                                                                                                                                 [mxz]
        //                                                                                                                                                 [myx]
        //                                                                                                                                                 [myz]
        //                                                                                                                                                 [mzx]
        //                                                                                                                                                 [mzy]
        //                                                                                                                                                 [g11]
        //                                                                                                                                                 [g12]
        //                                                                                                                                                 [g13]
        //                                                                                                                                                 [g21]
        //                                                                                                                                                 [g22]
        //                                                                                                                                                 [g23]
        //                                                                                                                                                 [g31]
        //                                                                                                                                                 [g32]
        //                                                                                                                                                 [g33]

        mFitter.setFunctionEvaluator(new LevenbergMarquardtMultiVariateFunctionEvaluator() {
            @Override
            public int getNumberOfDimensions() {
                // Input points are true angular rate + specific force coordinates
                return 2 * BodyKinematics.COMPONENTS;
            }

            @Override
            public int getNumberOfVariables() {
                // The multivariate function returns the components of measured angular rate
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final double[] initial = new double[GENERAL_UNKNOWNS];

                initial[0] = mInitialSx;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2161
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2366
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration solution.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurements()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurements()}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #getMinimumRequiredMeasurements()}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredMeasurements()) {
            throw new IllegalArgumentException();
        }

        mPreliminarySubsetSize = preliminarySubsetSize;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public abstract void calibrate() throws LockedException, NotReadyException,
            CalibrationException;

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    public abstract RobustEstimatorMethod getMethod();

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param method robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
File Line
com/irurueta/navigation/indoor/position/LinearRangingAndRssiPositionEstimator.java 80
com/irurueta/navigation/indoor/position/LinearRangingPositionEstimator.java 79
com/irurueta/navigation/indoor/position/LinearRssiPositionEstimator.java 82
            final RangingAndRssiPositionEstimatorListener<P> listener) {
        super(listener);
        init();
    }

    /**
     * Indicates whether an homogeneous linear solver is used to estimate position.
     *
     * @return true if homogeneous linear solver is used, false if an inhomogeneous
     * linear one is used instead.
     */
    public boolean isHomogeneousLinearSolverUsed() {
        return mUseHomogeneousLinearSolver;
    }

    /**
     * Specifies whether an homogeneous linear solver is used to estimate position.
     *
     * @param useHomogeneousLinearSolver true if homogeneous linear solver is used, false
     *                                   if an inhomogeneous linear one is used instead.
     * @throws LockedException if estimator is locked.
     */
    public void setHomogeneousLinearSolverUsed(final boolean useHomogeneousLinearSolver)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        mUseHomogeneousLinearSolver = useHomogeneousLinearSolver;
    }

    /**
     * Gets minimum required number of located radio sources to perform lateration.
     *
     * @return minimum required number of located radio sources to perform lateration.
     */
    @Override
    public int getMinRequiredSources() {
        return mUseHomogeneousLinearSolver ?
                mHomogeneousTrilaterationSolver.getMinRequiredPositionsAndDistances() :
                mInhomogeneousTrilaterationSolver.getMinRequiredPositionsAndDistances();
    }

    /**
     * Indicates whether estimator is ready to find a solution.
     *
     * @return true if estimator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return (!mUseHomogeneousLinearSolver && mInhomogeneousTrilaterationSolver.isReady()) ||
                (mUseHomogeneousLinearSolver && mHomogeneousTrilaterationSolver.isReady());
    }

    /**
     * Returns boolean indicating whether this estimator is locked because an estimation is already in progress.
     *
     * @return true if estimator is locked, false otherwise.
     */
    @Override
    public boolean isLocked() {
        return mInhomogeneousTrilaterationSolver.isLocked() ||
                mHomogeneousTrilaterationSolver.isLocked();
    }

    /**
     * Estimates position based on provided located radio sources and RSSI readings of
     * such radio sources at an unknown location.
     *
     * @throws LockedException             if estimator is locked.
     * @throws NotReadyException           if estimator is not ready.
     * @throws PositionEstimationException if estimation fails for some other reason.
     */
    @SuppressWarnings("Duplicates")
    @Override
    public void estimate() throws LockedException, NotReadyException,
            PositionEstimationException {
        try {
            if (mUseHomogeneousLinearSolver) {
                mHomogeneousTrilaterationSolver.solve();
                mEstimatedPositionCoordinates =
                        mHomogeneousTrilaterationSolver.getEstimatedPositionCoordinates();
            } else {
                mInhomogeneousTrilaterationSolver.solve();
                mEstimatedPositionCoordinates =
                        mInhomogeneousTrilaterationSolver.getEstimatedPositionCoordinates();
            }
        } catch (final LaterationException e) {
            throw new PositionEstimationException(e);
        }
    }

    /**
     * Gets known positions of radio sources used internally to solve lateration.
     *
     * @return known positions used internally.
     */
    @Override
    public P[] getPositions() {
        return mUseHomogeneousLinearSolver ?
                mHomogeneousTrilaterationSolver.getPositions() :
                mInhomogeneousTrilaterationSolver.getPositions();
    }

    /**
     * Gets euclidean distances from known located radio sources to
     * the location of provided readings in a fingerprint.
     * Distance values are used internally to solve lateration.
     *
     * @return euclidean distances used internally.
     */
    @Override
    public double[] getDistances() {
        return mUseHomogeneousLinearSolver ?
                mHomogeneousTrilaterationSolver.getDistances() :
                mInhomogeneousTrilaterationSolver.getDistances();
    }

    /**
     * Internally sets located radio sources used for lateration.
     *
     * @param sources located radio sources used for lateration.
     * @throws IllegalArgumentException if provided value is null or the number of
     *                                  provided sources is less than the required
     *                                  minimum.
     */
    protected void internalSetSources(final List<? extends RadioSourceLocated<P>> sources) {
        super.internalSetSources(sources);
        buildPositionsAndDistances();
    }

    /**
     * Internally sets fingerprint containing readings at an unknown location for provided
     * located radio sources.
     *
     * @param fingerprint fingerprint containing readings at an unknown location for
     *                    provided located radio sources.
     * @throws IllegalArgumentException if provided value is null.
     */
    protected void internalSetFingerprint(
            final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<? extends RadioSource>> fingerprint) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2050
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1695
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
            throw new IllegalArgumentException();
        }

        mPreliminarySubsetSize = preliminarySubsetSize;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public abstract void calibrate() throws LockedException, NotReadyException,
            CalibrationException;

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    public abstract RobustEstimatorMethod getMethod();

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param method robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1536
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1419
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
            throw new IllegalArgumentException();
        }

        mPreliminarySubsetSize = preliminarySubsetSize;
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public abstract void calibrate() throws LockedException, NotReadyException,
            CalibrationException;

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    public abstract RobustEstimatorMethod getMethod();

    /**
     * Creates a robust known frame magnetometer calibrator.
     *
     * @param method robust estimator method.
     * @return a robust known frame magnetometer calibrator.
     */
    public static RobustKnownFrameMagnetometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2320
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2428
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        mPreliminarySubsetSize = preliminarySubsetSize;
    }

    /**
     * Estimates magnetometer calibration parameters containing soft-iron
     * scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public abstract void calibrate() throws LockedException, NotReadyException,
            CalibrationException;

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    public abstract RobustEstimatorMethod getMethod();

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param method robust estimator method.
     * @return a robust magnetometer calibrator.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
File Line
com/irurueta/navigation/indoor/position/SequentialRobustMixedPositionEstimator.java 986
com/irurueta/navigation/indoor/position/SequentialRobustRangingAndRssiPositionEstimator.java 960
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator.java 1361
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator.java 1342
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0 (which
     * is equivalent to 100%) for robust position estimation on ranging data. The
     * amount of confidence indicates the probability that the estimated result is
     * correct. Usually this value will be close to 1.0, but not exactly 1.0.
     *
     * @return amount of confidence for robust position estimation as a value between
     * 0.0 and 1.0.
     */
    public double getRangingConfidence() {
        return mRangingConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%) for robust position estimation on ranging data. The amount
     * of confidence indicates the probability that the estimated result is correct.
     * Usually this value will be close to 1.0, but not exactly 1.0.
     *
     * @param rangingConfidence confidence to be set for robust position estimation as
     *                          a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if estimator is locked.
     */
    public void setRangingConfidence(final double rangingConfidence) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rangingConfidence < MIN_CONFIDENCE || rangingConfidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mRangingConfidence = rangingConfidence;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%) for robust position estimation on RSSI data. The amount of
     * confidence indicates the probability that the estimated result is correct.
     * Usually this value will be close to 1.0, but not exactly 1.0.
     *
     * @return amount of confidence for robust position estimation as a value between
     * 0.0 and 1.0.
     */
    public double getRssiConfidence() {
        return mRssiConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%) for robust position estimation on RSSI data. The amount
     * of confidence indicates the probability that the estimated result is correct.
     * Usually this value will be close to 1.0, but not exactly 1.0.
     *
     * @param rssiConfidence amount of confidence for robust position estimation as
     *                       a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if estimator is locked.
     */
    public void setRssiConfidence(final double rssiConfidence) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rssiConfidence < MIN_CONFIDENCE || rssiConfidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mRssiConfidence = rssiConfidence;
    }

    /**
     * Gets maximum allowed number of iterations for robust ranging position estimation.
     * When the maximum number of iterations is exceeded, an approximate result might
     * be available for retrieval.
     *
     * @return maximum allowed number of iterations for position estimation.
     */
    public int getRangingMaxIterations() {
        return mRangingMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations for robust ranging position
     * estimation.
     * When the maximum number of iterations is exceeded, an approximate result might
     * be available for retrieval.
     *
     * @param rangingMaxIterations maximum allowed number of iterations to be set for
     *                             position estimation.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if estimator is locked.
     */
    public void setRangingMaxIterations(final int rangingMaxIterations)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rangingMaxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mRangingMaxIterations = rangingMaxIterations;
    }

    /**
     * Gets maximum allowed number of iterations for robust RSSI position estimation.
     * When the maximum number of iterations is exceeded, an approximate result might
     * be available for retrieval.
     *
     * @return maximum allowed number of iterations for position estimation.
     */
    public int getRssiMaxIterations() {
        return mRssiMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations for robust RSSI position estimation.
     * When the maximum number of iterations is exceeded, an approximate result might
     * be available for retrieval.
     *
     * @param rssiMaxIterations maximum allowed number of iterations to be set for
     *                          position estimation.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if estimator is locked.
     */
    public void setRssiMaxIterations(final int rssiMaxIterations) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (rssiMaxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mRssiMaxIterations = rssiMaxIterations;
    }

    /**
     * Indicates whether result is refined using all found inliers.
     *
     * @return true if result is refined, false otherwise.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result is refined using all found inliers.
     *
     * @param refineResult true if result is refined, false otherwise.
     * @throws LockedException if this instance is locked.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Indicates whether a linear solver is used for preliminary solution estimation
     * using ranging measurements.
     * The result obtained on each preliminary solution might be later refined.
     *
     * @return true if a linear solver is used for preliminary solution estimation on
     * ranging readings.
     */
    public boolean isRangingLinearSolverUsed() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6317
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1719
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1089
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS ||
                initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMa.getElementAtIndex(0);
        mInitialMyx = initialMa.getElementAtIndex(1);
        mInitialMzx = initialMa.getElementAtIndex(2);

        mInitialMxy = initialMa.getElementAtIndex(3);
        mInitialSy = initialMa.getElementAtIndex(4);
        mInitialMzy = initialMa.getElementAtIndex(5);

        mInitialMxz = initialMa.getElementAtIndex(6);
        mInitialMyz = initialMa.getElementAtIndex(7);
        mInitialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 728
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 193
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Matrix> innerEstimator =
                new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices, final List<Matrix> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(final Matrix currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 190
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 733
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 196
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 192
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<PreliminaryResult>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mMeasurements.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return mPreliminarySubsetSize;
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<PreliminaryResult> solutions) {
                                computePreliminarySolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final PreliminaryResult currentEstimation, final int i) {
                                return computeError(mMeasurements.get(i), currentEstimation);
                            }

                            @Override
                            public boolean isReady() {
                                return RANSACRobustKnownFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1706
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 989
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1777
        mHardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMm(final Matrix initialMm)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS ||
                initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMm.getElementAtIndex(0);
        mInitialMyx = initialMm.getElementAtIndex(1);
        mInitialMzx = initialMm.getElementAtIndex(2);

        mInitialMxy = initialMm.getElementAtIndex(3);
        mInitialSy = initialMm.getElementAtIndex(4);
        mInitialMzy = initialMm.getElementAtIndex(5);

        mInitialMxz = initialMm.getElementAtIndex(6);
        mInitialMyz = initialMm.getElementAtIndex(7);
        mInitialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken.
     *
     * @return position where body magnetic flux density measurements have
     * been taken.
     */
    public NEDPosition getNedPosition() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 1722
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 964
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1793
        mInitialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMm(final Matrix initialMm)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS ||
                initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMm.getElementAtIndex(0);
        mInitialMyx = initialMm.getElementAtIndex(1);
        mInitialMzx = initialMm.getElementAtIndex(2);

        mInitialMxy = initialMm.getElementAtIndex(3);
        mInitialSy = initialMm.getElementAtIndex(4);
        mInitialMzy = initialMm.getElementAtIndex(5);

        mInitialMxz = initialMm.getElementAtIndex(6);
        mInitialMyz = initialMm.getElementAtIndex(7);
        mInitialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken.
     *
     * @return position where body magnetic flux density measurements have
     * been taken.
     */
    public NEDPosition getNedPosition() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3200
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2994
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredSequences()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredSequences}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than
     *                                  {@link #getMinimumRequiredSequences}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredSequences()) {
            throw new IllegalArgumentException();
        }

        mPreliminarySubsetSize = preliminarySubsetSize;
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    public abstract RobustEstimatorMethod getMethod();

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param method robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustEasyGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4388
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4606
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurements()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurements}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than
     *                                  {@link #getMinimumRequiredMeasurements}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredMeasurements()) {
            throw new IllegalArgumentException();
        }

        mPreliminarySubsetSize = preliminarySubsetSize;
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    public abstract RobustEstimatorMethod getMethod();

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param method robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 728
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 193
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 953
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Matrix> innerEstimator =
                new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices, final List<Matrix> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(final Matrix currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 190
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 733
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1856
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 196
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1886
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 192
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 954
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<PreliminaryResult>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mMeasurements.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return mPreliminarySubsetSize;
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<PreliminaryResult> solutions) {
                                computePreliminarySolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final PreliminaryResult currentEstimation, final int i) {
                                return computeError(mMeasurements.get(i), currentEstimation);
                            }

                            @Override
                            public boolean isReady() {
                                return RANSACRobustKnownFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2784
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4139
                && mSequences.size() >= getMinimumRequiredSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public abstract void calibrate() throws LockedException, NotReadyException,
            CalibrationException;

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2746
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4195
                && mSequences.size() >= getMinimumRequiredSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public abstract void calibrate() throws LockedException, NotReadyException,
            CalibrationException;

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6317
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6757
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1610
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1719
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1761
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS ||
                initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMa.getElementAtIndex(0);
        mInitialMyx = initialMa.getElementAtIndex(1);
        mInitialMzx = initialMa.getElementAtIndex(2);

        mInitialMxy = initialMa.getElementAtIndex(3);
        mInitialSy = initialMa.getElementAtIndex(4);
        mInitialMzy = initialMa.getElementAtIndex(5);

        mInitialMxz = initialMa.getElementAtIndex(6);
        mInitialMyz = initialMa.getElementAtIndex(7);
        mInitialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 363
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 377
            final KnownFrameAccelerometerLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Indicates whether calibrator is ready to start the calibration.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null
                && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator2D.java 2854
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator2D.java 871
    }

    /**
     * Gets estimated located radio source with estimated transmitted power.
     *
     * @return estimated located radio source with estimated transmitted power or null.
     */
    @Override
    @SuppressWarnings("unchecked")
    public RadioSourceWithPowerAndLocated<Point2D> getEstimatedRadioSource() {
        final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings = getReadings();
        if (readings == null || readings.isEmpty()) {
            return null;
        }
        final S source = readings.get(0).getSource();

        final Point2D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        final Double transmittedPowerVariance =
                getEstimatedTransmittedPowerVariance();
        final Double transmittedPowerStandardDeviation = transmittedPowerVariance != null ?
                Math.sqrt(transmittedPowerVariance) : null;

        final Double pathlossExponentVariance =
                getEstimatedPathLossExponentVariance();
        final Double pathlossExponentStandardDeviation = pathlossExponentVariance != null ?
                Math.sqrt(pathlossExponentVariance) : null;

        if (source instanceof WifiAccessPoint) {
            final WifiAccessPoint accessPoint = (WifiAccessPoint) source;
            return new WifiAccessPointWithPowerAndLocated2D(accessPoint.getBssid(),
                    source.getFrequency(), accessPoint.getSsid(),
                    getEstimatedTransmittedPowerdBm(),
                    transmittedPowerStandardDeviation,
                    getEstimatedPathLossExponent(),
                    pathlossExponentStandardDeviation,
                    estimatedPosition,
                    estimatedPositionCovariance);
        } else if (source instanceof Beacon) {
            final Beacon beacon = (Beacon) source;
            return new BeaconWithPowerAndLocated2D(beacon.getIdentifiers(),
                    getEstimatedTransmittedPowerdBm(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(),
                    getEstimatedPathLossExponent(),
                    transmittedPowerStandardDeviation,
                    pathlossExponentStandardDeviation,
                    estimatedPosition, estimatedPositionCovariance);
        } else {
            return null;
        }
    }

    /**
     * Solves preliminar solution for a subset of samples.
     *
     * @param samplesIndices indices of subset samples.
     * @param solutions      instance where solution will be stored.
     */
    @Override
    protected void solvePreliminarSolutions(
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator3D.java 2861
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator3D.java 869
    }

    /**
     * Gets estimated located radio source with estimated transmitted power.
     *
     * @return estimated located radio source with estimated transmitted power or null.
     */
    @Override
    @SuppressWarnings("unchecked")
    public RadioSourceWithPowerAndLocated<Point3D> getEstimatedRadioSource() {
        final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings = getReadings();
        if (readings == null || readings.isEmpty()) {
            return null;
        }
        final S source = readings.get(0).getSource();

        final Point3D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        final Double transmittedPowerVariance =
                getEstimatedTransmittedPowerVariance();
        final Double transmittedPowerStandardDeviation = transmittedPowerVariance != null ?
                Math.sqrt(transmittedPowerVariance) : null;

        final Double pathlossExponentVariance =
                getEstimatedPathLossExponentVariance();
        final Double pathlossExponentStandardDeviation = pathlossExponentVariance != null ?
                Math.sqrt(pathlossExponentVariance) : null;

        if (source instanceof WifiAccessPoint) {
            final WifiAccessPoint accessPoint = (WifiAccessPoint) source;
            return new WifiAccessPointWithPowerAndLocated3D(accessPoint.getBssid(),
                    source.getFrequency(), accessPoint.getSsid(),
                    getEstimatedTransmittedPowerdBm(),
                    transmittedPowerStandardDeviation,
                    getEstimatedPathLossExponent(),
                    pathlossExponentStandardDeviation,
                    estimatedPosition,
                    estimatedPositionCovariance);
        } else if (source instanceof Beacon) {
            final Beacon beacon = (Beacon) source;
            return new BeaconWithPowerAndLocated3D(beacon.getIdentifiers(),
                    getEstimatedTransmittedPowerdBm(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(),
                    getEstimatedPathLossExponent(),
                    transmittedPowerStandardDeviation,
                    pathlossExponentStandardDeviation,
                    estimatedPosition, estimatedPositionCovariance);
        } else {
            return null;
        }
    }

    /**
     * Solves preliminar solution for a subset of samples.
     *
     * @param samplesIndices indices of subset samples.
     * @param solutions      instance where solution will be stored.
     */
    @Override
    protected void solvePreliminarSolutions(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4235
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4468
            mRunning = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
        return mEstimatedChiSq;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                              if there are numerical errors.
     * @throws FittingException                              if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException      if fitter is not ready.
     * @throws InvalidSourceAndDestinationFrameTypeException never happens
     */
    private void calibrateCommonAxisAndGDependentCrossBiases()
            throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException,
            InvalidSourceAndDestinationFrameTypeException {

        // The gyroscope model is
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // For convergence purposes of the Levenberg-Marquardt algorithm, we
        // take common factor M = I + Mg

        // and the gyroscope model can be better expressed as:

        // Ωmeas = M*(Ωtrue + b + G * ftrue)

        // where:
        // bg = M*b --> b = M^-1*bg
        // Gg = M*G --> G = M^-1*Gg

        // We know that the norm of the true angular rate when the device is in a pixed
        // and unknown position and orientation is equal to the Earth rotation rate.
        // ||Ωtrue|| = 7.292115E-5 rad/s

        // Hence
        // Ωmeas - M*b - M*G*ftrue = M*Ωtrue
        // M^-1 * (Ωmeas - M*b - M*G*ftrue) = Ωtrue

        // ||Ωtrue||^2 = (M^-1 * (Ωmeas - M*b - M*G*ftrue))^T*(M^-1 * (Ωmeas - M*b - M*G*ftrue))
        // ||Ωtrue||^2 = (Ωmeas - M*b - M*G*ftrue)^T * (M^-1)^T * M^-1 * (Ωmeas - M*b - M*G*ftrue)
        // ||Ωtrue||^2 = (Ωmeas - M*b - M*G*ftrue)^T * ||M^-1||^2 * (Ωmeas - M*b - M*G*ftrue)
        // ||Ωtrue||^2 = ||Ωmeas - M*b - M*G*ftrue||^2 * ||M^-1||^2

        // Where:

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11 	m12 	m13]
        //     [0 		m22 	m23]
        //     [0 	 	0 		m33]

        // G = [g11 	g12 	g13]
        //     [g21 	g22 	g23]
        //     [g31 	g32 	g33]

        // ftrue = [ftruex]
        //         [ftruey]
        //         [fturez]

        final GradientEstimator gradientEstimator = new GradientEstimator(
                new MultiDimensionFunctionEvaluatorListener() {
                    @Override
                    public double evaluate(double[] point) throws EvaluationException {
File Line
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator2D.java 2838
com/irurueta/navigation/indoor/radiosource/RssiRadioSourceEstimator2D.java 434
    }

    /**
     * Gets estimated located radio source with estimated transmitted power.
     *
     * @return estimated located radio source with estimated transmitted power or null.
     */
    @Override
    @SuppressWarnings("unchecked")
    public RadioSourceWithPowerAndLocated<Point2D> getEstimatedRadioSource() {
        final List<? extends RssiReadingLocated<S, Point2D>> readings = getReadings();
        if (readings == null || readings.isEmpty()) {
            return null;
        }
        final S source = readings.get(0).getSource();

        final Point2D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        final Double transmittedPowerVariance =
                getEstimatedTransmittedPowerVariance();
        final Double transmittedPowerStandardDeviation = transmittedPowerVariance != null ?
                Math.sqrt(transmittedPowerVariance) : null;

        final Double pathlossExponentVariance =
                getEstimatedPathLossExponentVariance();
        final Double pathlossExponentStandardDeviation = pathlossExponentVariance != null ?
                Math.sqrt(pathlossExponentVariance) : null;

        if (source instanceof WifiAccessPoint) {
            final WifiAccessPoint accessPoint = (WifiAccessPoint) source;
            return new WifiAccessPointWithPowerAndLocated2D(accessPoint.getBssid(),
                    source.getFrequency(), accessPoint.getSsid(),
                    getEstimatedTransmittedPowerdBm(),
                    transmittedPowerStandardDeviation,
                    getEstimatedPathLossExponent(),
                    pathlossExponentStandardDeviation,
                    estimatedPosition,
                    estimatedPositionCovariance);
        } else if (source instanceof Beacon) {
            final Beacon beacon = (Beacon) source;
            return new BeaconWithPowerAndLocated2D(beacon.getIdentifiers(),
                    getEstimatedTransmittedPowerdBm(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(),
                    getEstimatedPathLossExponent(),
                    transmittedPowerStandardDeviation,
                    pathlossExponentStandardDeviation,
                    estimatedPosition, estimatedPositionCovariance);
        } else {
            return null;
        }
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7543
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5342
                        mFmeasZ = point[2];

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateGeneral(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];
        final double m21 = result[4];
        final double m31 = result[5];

        final double m12 = result[6];
        final double m22 = result[7];
        final double m32 = result[8];

        final double m13 = result[9];
        final double m23 = result[10];
        final double m33 = result[11];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
        b.setElementAtIndex(0, bx);
        b.setElementAtIndex(1, by);
        b.setElementAtIndex(2, bz);

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, m21);
        m.setElementAtIndex(2, m31);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, m32);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        setResult(m, b);
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws FittingException                         if Levenberg-Marquardt fails for numerical reasons.
     * @throws AlgebraException                         if there are numerical instabilities that prevent
     *                                                  matrix inversion.
     * @throws com.irurueta.numerical.NotReadyException never happens.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2795
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4232
                | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
        return mEstimatedChiSq;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxisAndGDependentCrossBiases()
            throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5611
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5963
    private double evaluate(final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33)
            throws EvaluationException {

        // Ωmeas = M*(Ωtrue + b)
        // Ωtrue = M^-1 * Ωmeas - b

        try {
            if (mMeasAngularRate == null) {
                mMeasAngularRate = new Matrix(
                        BodyKinematics.COMPONENTS, 1);
            }
            if (mM == null) {
                mM = new Matrix(BodyKinematics.COMPONENTS,
                        BodyKinematics.COMPONENTS);
            }
            if (mInvM == null) {
                mInvM = new Matrix(BodyKinematics.COMPONENTS,
                        BodyKinematics.COMPONENTS);
            }
            if (mB == null) {
                mB = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (mTrueAngularRate == null) {
                mTrueAngularRate = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            mMeasAngularRate.setElementAtIndex(0, mMeasAngularRateX);
            mMeasAngularRate.setElementAtIndex(1, mMeasAngularRateY);
            mMeasAngularRate.setElementAtIndex(2, mMeasAngularRateZ);

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);

            mB.setElementAtIndex(0, mBiasX);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1968
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2010
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2785
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2747
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4140
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4196
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/gnss/GNSSEstimation.java 435
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 1471
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 1644
    }

    /**
     * Gets x coordinate of estimated ECEF user position.
     *
     * @param result instance where x coordinate of estimated ECEF user position will be stored.
     */
    public void getDistanceX(final Distance result) {
        result.setValue(mX);
        result.setUnit(DistanceUnit.METER);
    }

    /**
     * Gets x coordinate of estimated ECEF user position.
     *
     * @return x coordinate of estimated ECEF user position.
     */
    public Distance getDistanceX() {
        return new Distance(mX, DistanceUnit.METER);
    }

    /**
     * Sets x coordinate of estimated ECEF user position.
     *
     * @param x x coordinate of estimated ECEF user position.
     */
    public void setDistanceX(final Distance x) {
        mX = DistanceConverter.convert(x.getValue().doubleValue(), x.getUnit(),
                DistanceUnit.METER);
    }

    /**
     * Gets y coordinate of estimated ECEF user position.
     *
     * @param result instance where y coordinate of estimated ECEF user position will be stored.
     */
    public void getDistanceY(final Distance result) {
        result.setValue(mY);
        result.setUnit(DistanceUnit.METER);
    }

    /**
     * Gets y coordinate of estimated ECEF user position.
     *
     * @return y coordinate of estimated ECEF user position.
     */
    public Distance getDistanceY() {
        return new Distance(mY, DistanceUnit.METER);
    }

    /**
     * Sets y coordinate of estimated ECEF user position.
     *
     * @param y y coordinate of estimated ECEF user position.
     */
    public void setDistanceY(final Distance y) {
        mY = DistanceConverter.convert(y.getValue().doubleValue(), y.getUnit(),
                DistanceUnit.METER);
    }

    /**
     * Gets z coordinate of estimated ECEF user position.
     *
     * @param result instance where z coordinate of estimated ECEF user position will be stored.
     */
    public void getDistanceZ(final Distance result) {
        result.setValue(mZ);
        result.setUnit(DistanceUnit.METER);
    }

    /**
     * Gets z coordinate of estimated ECEF user position.
     *
     * @return z coordinate of estimated ECEF user position.
     */
    public Distance getDistanceZ() {
        return new Distance(mZ, DistanceUnit.METER);
    }

    /**
     * Sets z coordinate of estimated ECEF user position.
     *
     * @param z z coordinate of estimated ECEF user position.
     */
    public void setDistanceZ(final Distance z) {
        mZ = DistanceConverter.convert(z.getValue().doubleValue(), z.getUnit(),
                DistanceUnit.METER);
    }

    /**
     * Sets coordinates of estimated ECEF user position.
     *
     * @param x x coordinate.
     * @param y y coordinate.
     * @param z z coordinate.
     */
    public void setPositionCoordinates(final Distance x, final Distance y, final Distance z) {
        setDistanceX(x);
        setDistanceY(y);
        setDistanceZ(z);
    }

    /**
     * Gets x coordinate of estimated ECEF user velocity.
     *
     * @param result instance where x coordinate of estimated ECEF user velocity will
     *               be stored.
     */
    public void getSpeedX(final Speed result) {
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 838
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 842
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 855
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 7127
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2617
                    mEstimatedMa.getElementAt(i, i) - 1.0);
        }

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters for the general case.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the general purpose case.
     *               Must have length 9.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneral(final double[] params) throws EvaluationException {
        final double m11 = params[0];
        final double m21 = params[1];
        final double m31 = params[2];

        final double m12 = params[3];
        final double m22 = params[4];
        final double m32 = params[5];

        final double m13 = params[6];
        final double m23 = params[7];
        final double m33 = params[8];

        return evaluate(m11, m21, m31, m12, m22, m32,
                m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters when common z-axis is assumed.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the common z-axis case.
     *               Must have length 6.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateCommonAxis(final double[] params) throws EvaluationException {
        final double m11 = params[0];

        final double m12 = params[1];
        final double m22 = params[2];

        final double m13 = params[3];
        final double m23 = params[4];
        final double m33 = params[5];

        return evaluate(m11, 0.0, 0.0, m12, m22, 0.0,
                m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param m11 element 1,1 of cross-coupling error matrix.
     * @param m21 element 2,1 of cross-coupling error matrix.
     * @param m31 element 3,1 of cross-coupling error matrix.
     * @param m12 element 1,2 of cross-coupling error matrix.
     * @param m22 element 2,2 of cross-coupling error matrix.
     * @param m32 element 3,2 of cross-coupling error matrix.
     * @param m13 element 1,3 of cross-coupling error matrix.
     * @param m23 element 2,3 of cross-coupling error matrix.
     * @param m33 element 3,3 of cross-coupling error matrix.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluate(final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33)
            throws EvaluationException {

        // fmeas = M*(ftrue + b)

        // ftrue = M^-1*fmeas - b

        try {
            if (mFmeas == null) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1343
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1402
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2157
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(
            final float progressDelta) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(
            final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(
            final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(
            final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(
            final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2019
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1265
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3156
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2119
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4562
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredSequences()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredSequences}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than
     *                                  {@link #getMinimumRequiredSequences}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredSequences()) {
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 330
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 329
            final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position for a radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3200
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4388
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredSequences()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredSequences}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than
     *                                  {@link #getMinimumRequiredSequences}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredSequences()) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2163
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2994
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1794
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4606
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration solution.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1866
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2141
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(
            final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(
            final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1977
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1939
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1240
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3409
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4926
                        return evaluateGeneralWithGDependentCrossBiases(mI, params);
                    }
                });

        final Matrix initialM = Matrix.identity(
                BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        initialM.add(getInitialMg());

        final Matrix invInitialM = Utils.inverse(initialM);
        final Matrix initialBg = getInitialBiasAsMatrix();
        final Matrix initialB = invInitialM.multiplyAndReturnNew(initialBg);
        final Matrix initialGg = getInitialGg();
        final Matrix initialG = invInitialM.multiplyAndReturnNew(initialGg);

        mFitter.setFunctionEvaluator(
                new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
                    @Override
                    public int getNumberOfDimensions() {
                        // Before and after normalized gravity versors
                        return 2 * BodyKinematics.COMPONENTS;
                    }

                    @Override
                    public double[] createInitialParametersArray() {
                        final double[] initial =
                                new double[GENERAL_UNKNOWNS_AND_CROSS_BIASES];

                        // biases b
                        for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                            initial[i] = initialB.getElementAtIndex(i);
                        }

                        // cross coupling errors M
                        final int num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                        for (int i = 0, j = BodyKinematics.COMPONENTS; i < num; i++, j++) {
                            initial[j] = initialM.getElementAtIndex(i);
                        }

                        // g-dependent cross biases G
                        for (int i = 0, j = BodyKinematics.COMPONENTS + num; i < num; i++, j++) {
                            initial[j] = initialG.getElementAtIndex(i);
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1866
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2019
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2794
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1939
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2756
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4149
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4205
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1240
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2157
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(
            final boolean refineResult) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(
            final boolean keepCovariance) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1977
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1343
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1402
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1265
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2141
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/INSGNSSLooselyCoupledKalmanFilteredEstimator.java 1922
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java 1640
            mState = new INSLooselyCoupledKalmanState();
        }
    }

    /**
     * Corrects provided kinematics by taking into account currently estimated
     * specific force and angular rate biases.
     * This method stores the result into the variable member containing corrected
     * kinematics values.
     *
     * @param kinematics kinematics instance to be corrected.
     */
    private void correctKinematics(final BodyKinematics kinematics) {
        if (mCorrectedKinematics == null) {
            mCorrectedKinematics = new BodyKinematics();
        }

        final double accelBiasX;
        final double accelBiasY;
        final double accelBiasZ;
        final double gyroBiasX;
        final double gyroBiasY;
        final double gyroBiasZ;
        if (mState != null) {
            accelBiasX = getValueOrZero(mState.getAccelerationBiasX());
            accelBiasY = getValueOrZero(mState.getAccelerationBiasY());
            accelBiasZ = getValueOrZero(mState.getAccelerationBiasZ());
            gyroBiasX = getValueOrZero(mState.getGyroBiasX());
            gyroBiasY = getValueOrZero(mState.getGyroBiasY());
            gyroBiasZ = getValueOrZero(mState.getGyroBiasZ());
        } else {
            accelBiasX = 0.0;
            accelBiasY = 0.0;
            accelBiasZ = 0.0;
            gyroBiasX = 0.0;
            gyroBiasY = 0.0;
            gyroBiasZ = 0.0;
        }

        final double fx = kinematics.getFx();
        final double fy = kinematics.getFy();
        final double fz = kinematics.getFz();
        final double angularRateX = kinematics.getAngularRateX();
        final double angularRateY = kinematics.getAngularRateY();
        final double angularRateZ = kinematics.getAngularRateZ();

        mCorrectedKinematics.setSpecificForceCoordinates(
                fx - accelBiasX, fy - accelBiasY, fz - accelBiasZ);
        mCorrectedKinematics.setAngularRateCoordinates(
                angularRateX - gyroBiasX,
                angularRateY - gyroBiasY,
                angularRateZ - gyroBiasZ);
    }

    /**
     * Returns provided value if not infinity and not NaN.
     *
     * @param value value to be returned.
     * @return value or 0.0.
     */
    private double getValueOrZero(final double value) {
        if (Double.isNaN(value) || Double.isInfinite(value)) {
            return 0.0;
        } else {
            return value;
        }
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 926
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1067
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyMagneticFluxDensity measurement : mMeasurements) {
            final BodyMagneticFluxDensity measuredMagneticFluxDensity =
                    measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final ECEFFrame ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final double year = measurement.getYear();

            final double latitude = nedFrame.getLatitude();
            final double longitude = nedFrame.getLongitude();
            final double height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body mangetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb,
                    expectedMagneticFluxDensity);

            final double bMeasX = measuredMagneticFluxDensity.getBx();
            final double bMeasY = measuredMagneticFluxDensity.getBy();
            final double bMeasZ = measuredMagneticFluxDensity.getBz();

            final double bTrueX = expectedMagneticFluxDensity.getBx();
            final double bTrueY = expectedMagneticFluxDensity.getBy();
            final double bTrueZ = expectedMagneticFluxDensity.getBz();

            a.setElementAt(i, 0, 1.0);
            a.setElementAt(i, 3, bTrueX);
            a.setElementAt(i, 6, bTrueY);
            a.setElementAt(i, 7, bTrueZ);

            b.setElementAtIndex(i, bMeasX - bTrueX);
            i++;

            a.setElementAt(i, 1, 1.0);
            a.setElementAt(i, 4, bTrueY);
            a.setElementAt(i, 8, bTrueZ);
File Line
com/irurueta/navigation/lateration/NonLinearLeastSquaresLateration2DSolver.java 395
com/irurueta/navigation/lateration/RobustLateration2DSolver.java 1417
    }

    /**
     * Internally sets circles defining positions and euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 2.
     */
    private void internalSetCircles(final Circle[] circles) {
        if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final Point2D[] positions = new Point2D[circles.length];
        final double[] distances = new double[circles.length];
        for (int i = 0; i < circles.length; i++) {
            final Circle circle = circles[i];
            positions[i] = circle.getCenter();
            distances[i] = circle.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }

    /**
     * Internally sets circles defining positions and euclidean distances along with the standard
     * deviations of provided circles radii.
     *
     * @param circles                  circles defining positions and distances.
     * @param radiusStandardDeviations standard deviations of circles radii.
     * @throws IllegalArgumentException if circles is null, length of arrays is less than
     *                                  2 or don't have the same length.
     */
    private void internalSetCirclesAndStandardDeviations(
            final Circle[] circles,
            final double[] radiusStandardDeviations) {
        if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        if (radiusStandardDeviations == null) {
            throw new IllegalArgumentException();
        }

        if (radiusStandardDeviations.length != circles.length) {
            throw new IllegalArgumentException();
        }

        final Point2D[] positions = new Point2D[circles.length];
        final double[] distances = new double[circles.length];
        for (int i = 0; i < circles.length; i++) {
            Circle circle = circles[i];
            positions[i] = circle.getCenter();
            distances[i] = circle.getRadius();
        }

        internalSetPositionsDistancesAndStandardDeviations(positions, distances,
                radiusStandardDeviations);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1093
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3106
        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }
File Line
com/irurueta/navigation/lateration/NonLinearLeastSquaresLateration3DSolver.java 404
com/irurueta/navigation/lateration/RobustLateration3DSolver.java 1428
    public void internalSetSpheres(final Sphere[] spheres) {
        if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final Point3D[] positions = new Point3D[spheres.length];
        final double[] distances = new double[spheres.length];
        for (int i = 0; i < spheres.length; i++) {
            final Sphere sphere = spheres[i];
            positions[i] = sphere.getCenter();
            distances[i] = sphere.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }

    /**
     * Internally sets spheres defining positions and euclidean distances along with the standard
     * deviations of provided spheres radii.
     *
     * @param spheres                  spheres defining positions and distances.
     * @param radiusStandardDeviations standard deviations of circles radii.
     * @throws IllegalArgumentException if spheres is null, length of arrays is less than
     *                                  2 or don't have the same length.
     */
    private void internalSetSpheresAndStandardDeviations(
            final Sphere[] spheres,
            final double[] radiusStandardDeviations) {
        if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        if (radiusStandardDeviations == null) {
            throw new IllegalArgumentException();
        }

        if (radiusStandardDeviations.length != spheres.length) {
            throw new IllegalArgumentException();
        }

        final Point3D[] positions = new Point3D[spheres.length];
        final double[] distances = new double[spheres.length];
        for (int i = 0; i < spheres.length; i++) {
            final Sphere sphere = spheres[i];
            positions[i] = sphere.getCenter();
            distances[i] = sphere.getRadius();
        }

        internalSetPositionsDistancesAndStandardDeviations(positions, distances,
                radiusStandardDeviations);

    }
File Line
com/irurueta/navigation/indoor/position/PROMedSRobustMixedPositionEstimator2D.java 367
com/irurueta/navigation/indoor/position/PROMedSRobustRangingAndRssiPositionEstimator2D.java 367
com/irurueta/navigation/indoor/position/PROMedSRobustRangingPositionEstimator2D.java 366
com/irurueta/navigation/indoor/position/PROMedSRobustRssiPositionEstimator2D.java 365
            final RobustMixedPositionEstimatorListener<Point2D> listener) {
        this(sources, fingerprint, listener);
        internalSetSourceQualityScores(sourceQualityScores);
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Returns quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each radio source.
     */
    public double[] getSourceQualityScores() {
        return mSourceQualityScores;
    }

    /**
     * Sets quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the radio source.
     *
     * @param sourceQualityScores quality scores corresponding to each radio source.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setSourceQualityScores(final double[] sourceQualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSourceQualityScores(sourceQualityScores);
    }

    /**
     * Gets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each reading within provided
     * fingerprint.
     */
    public double[] getFingerprintReadingsQualityScores() {
        return mFingerprintReadingsQualityScores;
    }

    /**
     * Sets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @param fingerprintReadingsQualityScores quality scores corresponding to each
     *                                         reading within provided fingerprint.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return ((PROMedSRobustLateration2DSolver) mLaterationSolver).
                getStopThreshold();
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        ((PROMedSRobustLateration2DSolver) mLaterationSolver).
                setStopThreshold(stopThreshold);
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Initializes robust lateration solver.
     */
    private void init() {
        mLaterationSolver = new PROMedSRobustLateration2DSolver(
                mTrilaterationSolverListener);
    }

    /**
     * Sets quality scores corresponding to each provided located radio source.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param sourceQualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetSourceQualityScores(final double[] sourceQualityScores) {
        if (sourceQualityScores == null ||
                sourceQualityScores.length < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mSourceQualityScores = sourceQualityScores;

        buildPositionsDistancesDistanceStandardDeviationsAndQualityScores();
    }

    /**
     * Sets quality scores corresponding to each provided reading within provided
     * fingerprint.
     * This method is used internally and does not check whether instance is locked
     * or not.
     *
     * @param fingerprintReadingsQualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores lengt is
     *                                  smaller than 3 samples.
     */
    private void internalSetFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) {
        if (fingerprintReadingsQualityScores == null ||
                fingerprintReadingsQualityScores.length < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mFingerprintReadingsQualityScores = fingerprintReadingsQualityScores;

        buildPositionsDistancesDistanceStandardDeviationsAndQualityScores();
    }
}
File Line
com/irurueta/navigation/indoor/position/PROMedSRobustMixedPositionEstimator3D.java 367
com/irurueta/navigation/indoor/position/PROMedSRobustRangingAndRssiPositionEstimator3D.java 367
com/irurueta/navigation/indoor/position/PROMedSRobustRangingPositionEstimator3D.java 366
            final RobustMixedPositionEstimatorListener<Point3D> listener) {
        this(sources, fingerprint, listener);
        internalSetSourceQualityScores(sourceQualityScores);
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Returns quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each radio source.
     */
    public double[] getSourceQualityScores() {
        return mSourceQualityScores;
    }

    /**
     * Sets quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the radio source.
     *
     * @param sourceQualityScores quality scores corresponding to each radio source.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setSourceQualityScores(final double[] sourceQualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSourceQualityScores(sourceQualityScores);
    }

    /**
     * Gets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each reading within provided
     * fingerprint.
     */
    public double[] getFingerprintReadingsQualityScores() {
        return mFingerprintReadingsQualityScores;
    }

    /**
     * Sets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @param fingerprintReadingsQualityScores quality scores corresponding to each
     *                                         reading within provided fingerprint.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return ((PROMedSRobustLateration3DSolver) mLaterationSolver).
                getStopThreshold();
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        ((PROMedSRobustLateration3DSolver) mLaterationSolver).
                setStopThreshold(stopThreshold);
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Initializes robust lateration solver.
     */
    private void init() {
        mLaterationSolver = new PROMedSRobustLateration3DSolver(
                mTrilaterationSolverListener);
    }

    /**
     * Sets quality scores corresponding to each provided located radio source.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param sourceQualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetSourceQualityScores(final double[] sourceQualityScores) {
        if (sourceQualityScores == null ||
                sourceQualityScores.length < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mSourceQualityScores = sourceQualityScores;

        buildPositionsDistancesDistanceStandardDeviationsAndQualityScores();
    }

    /**
     * Sets quality scores corresponding to each provided reading within provided
     * fingerprint.
     * This method is used internally and does not check whether instance is locked
     * or not.
     *
     * @param fingerprintReadingsQualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores lengt is
     *                                  smaller than 3 samples.
     */
    private void internalSetFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) {
        if (fingerprintReadingsQualityScores == null ||
                fingerprintReadingsQualityScores.length < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mFingerprintReadingsQualityScores = fingerprintReadingsQualityScores;

        buildPositionsDistancesDistanceStandardDeviationsAndQualityScores();
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator2D.java 2848
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator2D.java 881
        final List<? extends RssiReadingLocated<S, Point2D>> readings = getReadings();
        if (readings == null || readings.isEmpty()) {
            return null;
        }
        final S source = readings.get(0).getSource();

        final Point2D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        final Double transmittedPowerVariance =
                getEstimatedTransmittedPowerVariance();
        final Double transmittedPowerStandardDeviation = transmittedPowerVariance != null ?
                Math.sqrt(transmittedPowerVariance) : null;

        final Double pathlossExponentVariance =
                getEstimatedPathLossExponentVariance();
        final Double pathlossExponentStandardDeviation = pathlossExponentVariance != null ?
                Math.sqrt(pathlossExponentVariance) : null;

        if (source instanceof WifiAccessPoint) {
            final WifiAccessPoint accessPoint = (WifiAccessPoint) source;
            return new WifiAccessPointWithPowerAndLocated2D(accessPoint.getBssid(),
                    source.getFrequency(), accessPoint.getSsid(),
                    getEstimatedTransmittedPowerdBm(),
                    transmittedPowerStandardDeviation,
                    getEstimatedPathLossExponent(),
                    pathlossExponentStandardDeviation,
                    estimatedPosition,
                    estimatedPositionCovariance);
        } else if (source instanceof Beacon) {
            final Beacon beacon = (Beacon) source;
            return new BeaconWithPowerAndLocated2D(beacon.getIdentifiers(),
                    getEstimatedTransmittedPowerdBm(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(),
                    getEstimatedPathLossExponent(),
                    transmittedPowerStandardDeviation,
                    pathlossExponentStandardDeviation,
                    estimatedPosition, estimatedPositionCovariance);
        } else {
            return null;
        }
    }

    /**
     * Solves preliminar solution for a subset of samples.
     *
     * @param samplesIndices indices of subset samples.
     * @param solutions      instance where solution will be stored.
     */
    @Override
    protected void solvePreliminarSolutions(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3008
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4235
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
        return mEstimatedChiSq;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxisAndGDependentCrossBiases()
            throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2798
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4468
            mRunning = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
        return mEstimatedChiSq;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxisAndGDependentCrossBiases()
            throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7547
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3791
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5346
                        return evaluateGeneral(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];
        final double m21 = result[4];
        final double m31 = result[5];

        final double m12 = result[6];
        final double m22 = result[7];
        final double m32 = result[8];

        final double m13 = result[9];
        final double m23 = result[10];
        final double m33 = result[11];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
        b.setElementAtIndex(0, bx);
        b.setElementAtIndex(1, by);
        b.setElementAtIndex(2, bz);

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, m21);
        m.setElementAtIndex(2, m31);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, m32);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        setResult(m, b);
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws FittingException                         if Levenberg-Marquardt fails for numerical reasons.
     * @throws AlgebraException                         if there are numerical instabilities that prevent
     *                                                  matrix inversion.
     * @throws com.irurueta.numerical.NotReadyException never happens.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
File Line
com/irurueta/navigation/indoor/radiosource/RobustRadioSourceEstimator.java 219
com/irurueta/navigation/lateration/RobustLaterationSolver.java 436
    }

    /**
     * Indicates whether estimator is locked during estimation.
     *
     * @return true if estimator is locked, false otherwise.
     */
    public boolean isLocked() {
        return mLocked;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * estimation.
     *
     * @return amount of progress variation before notifying a progress change during
     * estimation.
     */
    public float getProgressDelta() {
        return mProgressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * estimation.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during estimation.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if this estimator is locked.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA ||
                progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        mProgressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return mConfidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if estimator is locked.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        mConfidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling estimate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return mMaxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if this estimator is locked.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        mMaxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return mInliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return mRefineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if estimator is locked.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mRefineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return mKeepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mKeepCovariance = keepCovariance;
    }

    /**
     * Gets signal readings belonging to the same radio source.
     *
     * @return signal readings belonging to the same radio source.
     */
    public List<? extends R> getReadings() {
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator.java 473
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator.java 469
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, P> listener) {
        this(readings, initialPosition, initialTransmittedPowerdBm,
                listener);
        mInitialPathLossExponent = initialPathLossExponent;
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPowerdBm() {
        return mInitialTransmittedPowerdBm;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     *
     * @param initialTransmittedPowerdBm initial transmitted power to start the
     *                                   estimation of radio source transmitted
     *                                   power.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialTransmittedPowerdBm(final Double initialTransmittedPowerdBm)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialTransmittedPowerdBm = initialTransmittedPowerdBm;
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPower() {
        return mInitialTransmittedPowerdBm != null ?
                Utils.dBmToPower(mInitialTransmittedPowerdBm) : null;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     *
     * @param initialTransmittedPower initial transmitted power to start the
     *                                estimation of radio source transmitted power.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setInitialTransmittedPower(final Double initialTransmittedPower)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (initialTransmittedPower != null) {
            if (initialTransmittedPower < 0.0) {
                throw new IllegalArgumentException();
            }
            mInitialTransmittedPowerdBm = Utils.powerTodBm(
                    initialTransmittedPower);
        } else {
            mInitialTransmittedPowerdBm = null;
        }
    }

    /**
     * Gets initial position to start the estimation of radio source position.
     * If not defined, centroid of provided fingerprints will be used.
     *
     * @return initial position to start the estimation of radio source position.
     */
    public P getInitialPosition() {
        return mInitialPosition;
    }

    /**
     * Sets initial position to start the estimation of radio source position.
     * If not defined, centroid of provided fingerprints will be used.
     *
     * @param initialPosition initial position to start the estimation of radio
     *                        source position.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialPosition(final P initialPosition) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialPosition = initialPosition;
    }

    /**
     * Gets initial exponent typically used on free space for path loss propagation
     * in terms of distance.
     * On different environments path loss exponent might have different value:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     * <p>
     * If path loss exponent estimation is enabled, estimation will start at this
     * value and will converge to the most appropriate value.
     * If path loss exponent estimation is disabled, this value will be assumed
     * to be exact and the estimated path loss exponent will be equal to this
     * value.
     *
     * @return initial path loss exponent.
     */
    public double getInitialPathLossExponent() {
        return mInitialPathLossExponent;
    }

    /**
     * Sets initial exponent typically used on free space for path loss propagation
     * in terms of distance.
     * On different environments path loss exponent might have different value:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     * <p>
     * If path loss exponent estimation is enabled, estimation will start at this
     * value and will converge to the most appropriate value.
     * If path loss exponent estimation is disabled, this value will be assumed
     * to be exact and the estimated path loss exponent will be equal to this
     * value.
     *
     * @param initialPathLossExponent initial path loss exponent.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialPathLossExponent(final double initialPathLossExponent)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialPathLossExponent = initialPathLossExponent;
    }

    /**
     * Indicates whether transmitted power estimation is enabled or not.
     *
     * @return true if transmitted power estimation is enabled, false otherwise.
     */
    public boolean isTransmittedPowerEstimationEnabled() {
        return mTransmittedPowerEstimationEnabled;
    }

    /**
     * Specifies whether transmitted power estimation is enabled or not.
     *
     * @param transmittedPowerEstimationEnabled true if transmitted power estimation is enabled,
     *                                          false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setTransmittedPowerEstimationEnabled(
            final boolean transmittedPowerEstimationEnabled)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mTransmittedPowerEstimationEnabled = transmittedPowerEstimationEnabled;
    }

    /**
     * Indicates whether path loss estimation is enabled or not.
     *
     * @return true if path loss estimation is enabled, false otherwise.
     */
    public boolean isPathLossEstimationEnabled() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2050
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2366
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2161
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1695
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration solution.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurements()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurements()}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #getMinimumRequiredMeasurements()}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredMeasurements()) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1536
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2320
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1419
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2428
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return mPreliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 840
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 330
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 937
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 429
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 957
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Solution<Point2D>> innerEstimator =
                new PROSACRobustEstimator<>(
                        new PROSACRobustEstimatorListener<Solution<Point2D>>() {

                            @Override
                            public double[] getQualityScores() {
                                return mQualityScores;
                            }

                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point2D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point2D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return PROSACRobustRangingAndRssiRadioSourceEstimator2D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 844
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 331
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 857
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Solution<Point3D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator2D.java 2864
com/irurueta/navigation/indoor/radiosource/RssiRadioSourceEstimator2D.java 444
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator2D.java 881
        final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings = getReadings();
        if (readings == null || readings.isEmpty()) {
            return null;
        }
        final S source = readings.get(0).getSource();

        final Point2D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        final Double transmittedPowerVariance =
                getEstimatedTransmittedPowerVariance();
        final Double transmittedPowerStandardDeviation = transmittedPowerVariance != null ?
                Math.sqrt(transmittedPowerVariance) : null;

        final Double pathlossExponentVariance =
                getEstimatedPathLossExponentVariance();
        final Double pathlossExponentStandardDeviation = pathlossExponentVariance != null ?
                Math.sqrt(pathlossExponentVariance) : null;

        if (source instanceof WifiAccessPoint) {
            final WifiAccessPoint accessPoint = (WifiAccessPoint) source;
            return new WifiAccessPointWithPowerAndLocated2D(accessPoint.getBssid(),
                    source.getFrequency(), accessPoint.getSsid(),
                    getEstimatedTransmittedPowerdBm(),
                    transmittedPowerStandardDeviation,
                    getEstimatedPathLossExponent(),
                    pathlossExponentStandardDeviation,
                    estimatedPosition,
                    estimatedPositionCovariance);
        } else if (source instanceof Beacon) {
            final Beacon beacon = (Beacon) source;
            return new BeaconWithPowerAndLocated2D(beacon.getIdentifiers(),
                    getEstimatedTransmittedPowerdBm(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(),
                    getEstimatedPathLossExponent(),
                    transmittedPowerStandardDeviation,
                    pathlossExponentStandardDeviation,
                    estimatedPosition, estimatedPositionCovariance);
        } else {
            return null;
        }
    }
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator3D.java 2871
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator3D.java 2850
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator3D.java 879
        final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings = getReadings();
        if (readings == null || readings.isEmpty()) {
            return null;
        }
        final S source = readings.get(0).getSource();

        final Point3D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        final Double transmittedPowerVariance =
                getEstimatedTransmittedPowerVariance();
        final Double transmittedPowerStandardDeviation = transmittedPowerVariance != null ?
                Math.sqrt(transmittedPowerVariance) : null;

        final Double pathlossExponentVariance =
                getEstimatedPathLossExponentVariance();
        final Double pathlossExponentStandardDeviation = pathlossExponentVariance != null ?
                Math.sqrt(pathlossExponentVariance) : null;

        if (source instanceof WifiAccessPoint) {
            final WifiAccessPoint accessPoint = (WifiAccessPoint) source;
            return new WifiAccessPointWithPowerAndLocated3D(accessPoint.getBssid(),
                    source.getFrequency(), accessPoint.getSsid(),
                    getEstimatedTransmittedPowerdBm(),
                    transmittedPowerStandardDeviation,
                    getEstimatedPathLossExponent(),
                    pathlossExponentStandardDeviation,
                    estimatedPosition,
                    estimatedPositionCovariance);
        } else if (source instanceof Beacon) {
            final Beacon beacon = (Beacon) source;
            return new BeaconWithPowerAndLocated3D(beacon.getIdentifiers(),
                    getEstimatedTransmittedPowerdBm(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(),
                    getEstimatedPathLossExponent(),
                    transmittedPowerStandardDeviation,
                    pathlossExponentStandardDeviation,
                    estimatedPosition, estimatedPositionCovariance);
        } else {
            return null;
        }
    }
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator2D.java 375
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator2D.java 387
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if robust estimator is locked because an
     *                                  estimation is already in progress.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<Solution<Point2D>> innerEstimator =
                new MSACRobustEstimator<>(
                        new MSACRobustEstimatorListener<Solution<Point2D>>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point2D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point2D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return MSACRobustRangingAndRssiRadioSourceEstimator2D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator3D.java 375
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator3D.java 389
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if robust estimator is locked because an
     *                                  estimation is already in progress.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<Solution<Point3D>> innerEstimator =
                new MSACRobustEstimator<>(
                        new MSACRobustEstimatorListener<Solution<Point3D>>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point3D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point3D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return MSACRobustRangingAndRssiRadioSourceEstimator3D.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 349
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 354
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 353
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 354
            final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<PreliminaryResult> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1575
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1580
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3982
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3940
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3999
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4045
            final Collection<StandardDeviationBodyKinematics> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return mPosition;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final NEDPosition result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (mPosition != null) {
            final NEDVelocity velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    mPosition.getX(), mPosition.getY(), mPosition.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Indicates whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @return true if G-dependent cross biases will be estimated,
     * false otherwise.
     */
    public boolean isGDependentCrossBiasesEstimated() {
        return mEstimateGDependentCrossBiases;
    }

    /**
     * Specifies whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setGDependentCrossBiasesEstimated(
            final boolean estimateGDependentCrossBiases)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mEstimateGDependentCrossBiases = estimateGDependentCrossBiases;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public KnownBiasTurntableGyroscopeCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1793
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3698
        } catch (final AlgebraException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7706
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5183
                        mFmeasZ = point[2];

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateCommonAxis(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];

        final double m12 = result[4];
        final double m22 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
        b.setElementAtIndex(0, bx);
        b.setElementAtIndex(1, by);
        b.setElementAtIndex(2, bz);

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, 0.0);
        m.setElementAtIndex(2, 0.0);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        setResult(m, b);
    }

    /**
     * Makes proper conversion of internal cross-coupling and bias matrices.
     *
     * @param m internal cross-coupling matrix.
     * @param b internal bias matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m, final Matrix b) throws AlgebraException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1086
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2725
        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3652
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5187
                        return evaluateCommonAxis(i, params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];

        final double m12 = result[4];
        final double m22 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
        b.setElementAtIndex(0, bx);
        b.setElementAtIndex(1, by);
        b.setElementAtIndex(2, bz);

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, 0.0);
        m.setElementAtIndex(2, 0.0);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        setResult(m, b);
    }

    /**
     * Internal method to perform general calibration when G-dependant cross
     * biases are ignored.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneral()
            throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1575
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 350
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 355
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 354
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1580
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 355
        super(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<PreliminaryResult> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2060
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2057
        super(position, measurements, commonAxisUsed, hardIron,
                initialMm, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates magnetometer calibration parameters containing soft-iron
     * scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/frames/NEDFrame.java 883
com/irurueta/navigation/inertial/NEDVelocity.java 186
        return new Speed(getVelocityNorm(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along North axis.
     *
     * @param result instance where North velocity coordinate will be stored.
     */
    public void getSpeedN(final Speed result) {
        result.setValue(mVn);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along North axis.
     *
     * @return North velocity coordinate.
     */
    public Speed getSpeedN() {
        return new Speed(mVn, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along North axis.
     *
     * @param speedN North velocity coordinate to be set.
     */
    public void setSpeedN(final Speed speedN) {
        mVn = SpeedConverter.convert(speedN.getValue().doubleValue(),
                speedN.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along East axis.
     *
     * @param result instance where East velocity coordinate will be stored.
     */
    public void getSpeedE(final Speed result) {
        result.setValue(mVe);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along East axis.
     *
     * @return East velocity coordinate.
     */
    public Speed getSpeedE() {
        return new Speed(mVe, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along East axis.
     *
     * @param speedE East velocity coordinate to be set.
     */
    public void setSpeedE(final Speed speedE) {
        mVe = SpeedConverter.convert(speedE.getValue().doubleValue(),
                speedE.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along Down axis.
     *
     * @param result instance where Down velocity coordinate will be stored.
     */
    public void getSpeedD(final Speed result) {
        result.setValue(mVd);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along Down axis.
     *
     * @return Down velocity coordinate.
     */
    public Speed getSpeedD() {
        return new Speed(mVd, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along Down axis.
     *
     * @param speedD Down velocity coordinate to be set.
     */
    public void setSpeedD(final Speed speedD) {
        mVd = SpeedConverter.convert(speedD.getValue().doubleValue(),
                speedD.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets velocity coordinates of body frame resolved along North, East, Down
     * axes.
     *
     * @param speedN North velocity coordinate.
     * @param speedE East velocity coordinate.
     * @param speedD Down velocity coordinate.
     */
    public void setSpeedCoordinates(final Speed speedN, final Speed speedE,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1575
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3843
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3906
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 355
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2058
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 350
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1580
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 355
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 354
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2061
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<PreliminaryResult> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1096
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3436
            mRunning = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1387
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 929
        final double g33 = unknowns.getElementAtIndex(14);

        fillMg(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
        fillGg(g11, g12, g13, g21, g22, g23, g31, g32, g33);
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException if there are numerical errors.
     */
    private void calibrateGeneral() throws AlgebraException {
        // The gyroscope model is:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Hence:
        // [Ωmeasx] = [bx] + ( [1   0   0] + [sx    mxy    mxz]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0   1   0]   [myx   sy     myz]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0   0   1]   [mzx   mzy    sz ]  [Ωtruez]   [g31   g32   g33][ftruez]

        // [Ωmeasx] = [bx] + ( [1+sx  mxy    mxz ]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [myx   1+sy   myz ]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [mzx   mzy    1+sz]  [Ωtruez]   [g31   g32   g33][ftruez]

        // Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + myx * Ωtruex + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy, g11, g12, g13, g21, g22, g23,
        // g31, g32, g33
        // Reordering:
        // Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + myx * Ωtruex + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Ωmeasx - Ωtruex - bx = sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy - Ωtruey - by = myx * Ωtruex + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz - Ωtruez - bz = mzx * Ωtruex + mzy * Ωtruey + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // [Ωtruex  0       0       Ωtruey  Ωtruez  0       0       0       0       ftruex  ftruey  ftruez  0       0       0       0       0       0     ][sx ] =  [Ωmeasx - Ωtruex - bx]
        // [0       Ωtruey  0       0       0       Ωtruex  Ωtruez  0       0       0       0       0       ftruex  ftruey  ftruez  0       0       0     ][sy ]    [Ωmeasy - Ωtruey - by]
        // [0       0       Ωtruez  0       0       0       0       Ωtruex  Ωtruey  0       0       0       0       0       0       ftruex  ftruey  ftruez][sz ]    [Ωmeasz - Ωtruez - bz]
        //                                                                                                                                                 [mxy]
        //                                                                                                                                                 [mxz]
        //                                                                                                                                                 [myx]
        //                                                                                                                                                 [myz]
        //                                                                                                                                                 [mzx]
        //                                                                                                                                                 [mzy]
        //                                                                                                                                                 [g11]
        //                                                                                                                                                 [g12]
        //                                                                                                                                                 [g13]
        //                                                                                                                                                 [g21]
        //                                                                                                                                                 [g22]
        //                                                                                                                                                 [g23]
        //                                                                                                                                                 [g31]
        //                                                                                                                                                 [g32]
        //                                                                                                                                                 [g33]

        final BodyKinematics expectedKinematics = new BodyKinematics();

        final int rows = EQUATIONS_PER_MEASUREMENT * mMeasurements.size();
        final Matrix a = new Matrix(rows, GENERAL_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double omegaMeasX = measuredKinematics.getAngularRateX();
            final double omegaMeasY = measuredKinematics.getAngularRateY();
            final double omegaMeasZ = measuredKinematics.getAngularRateZ();

            final double omegaTrueX = expectedKinematics.getAngularRateX();
            final double omegaTrueY = expectedKinematics.getAngularRateY();
            final double omegaTrueZ = expectedKinematics.getAngularRateZ();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, omegaTrueX);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3109
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 627
            mRunning = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1575
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1679
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 350
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1697
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1580
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3843
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 355
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3906
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 354
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 355
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2061
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2058
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8396
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9807
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8819
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10312
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5483
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5824
    private double evaluate(final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33,
                            final double g11, final double g21, final double g31,
                            final double g12, final double g22, final double g32,
                            final double g13, final double g23, final double g33)
            throws EvaluationException {

        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
        // Ωmeas = M*(Ωtrue + b + G * ftrue)

        // M = I + Mg
        // bg = M*b --> b = M^-1*bg
        // Gg = M*G --> G = M^-1*Gg

        // Ωtrue = M^-1 * Ωmeas - b - G*ftrue

        try {
            if (mMeasAngularRate == null) {
                mMeasAngularRate = new Matrix(
                        BodyKinematics.COMPONENTS, 1);
            }
            if (mFmeas == null) {
                mFmeas = new Matrix(
                        BodyKinematics.COMPONENTS, 1);
            }
            if (mM == null) {
                mM = new Matrix(BodyKinematics.COMPONENTS,
                        BodyKinematics.COMPONENTS);
            }
            if (mInvM == null) {
                mInvM = new Matrix(BodyKinematics.COMPONENTS,
                        BodyKinematics.COMPONENTS);
            }
            if (mB == null) {
                mB = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (mG == null) {
                mG = new Matrix(BodyKinematics.COMPONENTS,
                        BodyKinematics.COMPONENTS);
            }
            if (mTrueAngularRate == null) {
                mTrueAngularRate = new Matrix(
                        BodyKinematics.COMPONENTS, 1);
            }
            if (mFtrue == null) {
                mFtrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (mTmp == null) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2014
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2156
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyMagneticFluxDensity measurement : mMeasurements) {
            final BodyMagneticFluxDensity measuredMagneticFluxDensity =
                    measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final ECEFFrame ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final double year = measurement.getYear();

            final double latitude = nedFrame.getLatitude();
            final double longitude = nedFrame.getLongitude();
            final double height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body mangetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb,
                    expectedMagneticFluxDensity);

            final double bMeasX = measuredMagneticFluxDensity.getBx();
            final double bMeasY = measuredMagneticFluxDensity.getBy();
            final double bMeasZ = measuredMagneticFluxDensity.getBz();

            final double bTrueX = expectedMagneticFluxDensity.getBx();
            final double bTrueY = expectedMagneticFluxDensity.getBy();
            final double bTrueZ = expectedMagneticFluxDensity.getBz();

            a.setElementAt(i, 0, bTrueX);
            a.setElementAt(i, 1, 0.0);
            a.setElementAt(i, 2, 0.0);
            a.setElementAt(i, 3, bTrueY);
            a.setElementAt(i, 4, bTrueZ);
            a.setElementAt(i, 5, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4139
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4203
            final KnownBiasTurntableGyroscopeCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    public int getMinimumRequiredMeasurements() {
        if (mCommonAxisUsed) {
            if (mEstimateGDependentCrossBiases) {
                return MINIMUM_MEASUREMENTS_COMMON_Z_AXIS_AND_CROSS_BIASES;
            } else {
                return MINIMUM_MEASUREMENTS_COMMON_Z_AXIS;
            }
        } else {
            if (mEstimateGDependentCrossBiases) {
                return MINIMUM_MEASUREMENTS_GENERAL_AND_CROSS_BIASES;
            } else {
                return MINIMUM_MEASUREMENTS_GENERAL;
            }
        }
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    public boolean isReady() {
        return mMeasurements != null
                && mMeasurements.size() >= getMinimumRequiredMeasurements();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                if (mEstimateGDependentCrossBiases) {
                    calibrateCommonAxisAndGDependentCrossBiases();
                } else {
                    calibrateCommonAxis();
                }
            } else {
                if (mEstimateGDependentCrossBiases) {
                    calibrateGeneralAndGDependentCrossBiases();
                } else {
                    calibrateGeneral();
                }
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException
                | com.irurueta.numerical.NotReadyException |
                InvalidSourceAndDestinationFrameTypeException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
File Line
com/irurueta/navigation/frames/ECIorECEFFrame.java 431
com/irurueta/navigation/gnss/GNSSEstimation.java 534
        return new Speed(getVelocityNorm(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets x coordinate of velocity of body frame resolved along ECEF-frame axes.
     *
     * @param result instance where x coordinate of velocity will be stored.
     */
    public void getSpeedX(final Speed result) {
        result.setValue(mVx);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets x coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @return x coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     */
    public Speed getSpeedX() {
        return new Speed(mVx, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets x coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @param speedX x coordinate of velocity of body frame resolved along ECI or ECEF-frame
     *               axes to be set.
     */
    public void setSpeedX(final Speed speedX) {
        mVx = SpeedConverter.convert(speedX.getValue().doubleValue(),
                speedX.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets y coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @param result instance where y coordinate of velocity will be stored.
     */
    public void getSpeedY(final Speed result) {
        result.setValue(mVy);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets y coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @return y coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     */
    public Speed getSpeedY() {
        return new Speed(mVy, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets y coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @param speedY y coordinate of velocity of body frame resolved along ECI or ECEF-frame
     *               axes to be set.
     */
    public void setSpeedY(final Speed speedY) {
        mVy = SpeedConverter.convert(speedY.getValue().doubleValue(),
                speedY.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets z coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @param result instance where z coordinate of velocity will be stored.
     */
    public void getSpeedZ(final Speed result) {
        result.setValue(mVz);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets z coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @return z coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     */
    public Speed getSpeedZ() {
        return new Speed(mVz, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets z coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @param speedZ z coordinate of velocity of body frame resolved along ECI or ECEF-frame
     *               axes to be set.
     */
    public void setSpeedZ(final Speed speedZ) {
        mVz = SpeedConverter.convert(speedZ.getValue().doubleValue(),
                speedZ.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets velocity coordinates of body frame resolved along ECI or ECEF-frame axes.
     *
     * @param speedX x coordinate of velocity to be set.
     * @param speedY y coordinate of velocity to be set.
     * @param speedZ z coordinate of velocity to be set.
     */
    public void setSpeedCoordinates(final Speed speedX,
File Line
com/irurueta/navigation/gnss/ECEFPositionAndVelocity.java 661
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 1341
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 1514
    }

    /**
     * Gets x coordinate of velocity resolved in ECEF axes.
     *
     * @param result instance where x coordinate of velocity will be stored.
     */
    public void getSpeedX(final Speed result) {
        result.setValue(mVx);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets x coordinate of velocity resolved in ECEF axes.
     *
     * @return x coordinate of velocity.
     */
    public Speed getSpeedX() {
        return new Speed(mVx, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets x coordinate of velocity resolved in ECEF axes.
     *
     * @param vx x coordinate of velocity.
     */
    public void setSpeedX(final Speed vx) {
        mVx = SpeedConverter.convert(vx.getValue().doubleValue(),
                vx.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets y coordinate of velocity resolved in ECEF axes.
     *
     * @param result instance where y coordinate of velocity will be stored.
     */
    public void getSpeedY(final Speed result) {
        result.setValue(mVy);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets y coordinate of velocity resolved in ECEF axes.
     *
     * @return y coordinate of velocity.
     */
    public Speed getSpeedY() {
        return new Speed(mVy, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets y coordinate of velocity resolved in ECEF axes.
     *
     * @param vy y coordinate of velocity.
     */
    public void setSpeedY(final Speed vy) {
        mVy = SpeedConverter.convert(vy.getValue().doubleValue(),
                vy.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets z coordinate of velocity resolved in ECEF axes.
     *
     * @param result instance where z coordinate of velocity will be stored.
     */
    public void getSpeedZ(final Speed result) {
        result.setValue(mVz);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets z coordinate of velocity resolved in ECEF axes.
     *
     * @return z coordinate of velocity.
     */
    public Speed getSpeedZ() {
        return new Speed(mVz, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets z coordinate of velocity resolved in ECEF axes.
     *
     * @param vz z coordinate of velocity.
     */
    public void setSpeedZ(final Speed vz) {
        mVz = SpeedConverter.convert(vz.getValue().doubleValue(),
                vz.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets coordinates of velocity resolved in ECEF axes.
     *
     * @param vx x coordinate of velocity.
     * @param vy y coordinate of velocity.
     * @param vz z coordinate of velocity.
     */
    public void setSpeedCoordinates(final Speed vx, final Speed vy, final Speed vz) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2743
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2701
            final EasyGyroscopeCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Gets minimum number of required sequences.
     *
     * @return minimum number of required sequences.
     */
    public int getMinimumRequiredSequences() {
        if (mCommonAxisUsed) {
            if (mEstimateGDependentCrossBiases) {
                return MINIMUM_SEQUENCES_COMMON_Z_AXIS_AND_CROSS_BIASES;
            } else {
                return MINIMUM_SEQUENCES_COMMON_Z_AXIS;
            }
        } else {
            if (mEstimateGDependentCrossBiases) {
                return MINIMUM_SEQUENCES_GENERAL_AND_CROSS_BIASES;
            } else {
                return MINIMUM_SEQUENCES_GENERAL;
            }
        }
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    public boolean isReady() {
        return mSequences != null
                && mSequences.size() >= getMinimumRequiredSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                if (mEstimateGDependentCrossBiases) {
                    calibrateCommonAxisAndGDependentCrossBiases();
                } else {
                    calibrateCommonAxis();
                }
            } else {
                if (mEstimateGDependentCrossBiases) {
                    calibrateGeneralAndGDependentCrossBiases();
                } else {
                    calibrateGeneral();
                }
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final FittingException
                | AlgebraException
                | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/indoor/position/PROMedSRobustMixedPositionEstimator3D.java 370
com/irurueta/navigation/indoor/position/PROMedSRobustRangingAndRssiPositionEstimator3D.java 370
com/irurueta/navigation/indoor/position/PROMedSRobustRangingPositionEstimator3D.java 369
com/irurueta/navigation/indoor/position/PROMedSRobustRssiPositionEstimator3D.java 373
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Returns quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each radio source.
     */
    public double[] getSourceQualityScores() {
        return mSourceQualityScores;
    }

    /**
     * Sets quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the radio source.
     *
     * @param sourceQualityScores quality scores corresponding to each radio source.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setSourceQualityScores(final double[] sourceQualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSourceQualityScores(sourceQualityScores);
    }

    /**
     * Gets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each reading within provided
     * fingerprint.
     */
    public double[] getFingerprintReadingsQualityScores() {
        return mFingerprintReadingsQualityScores;
    }

    /**
     * Sets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @param fingerprintReadingsQualityScores quality scores corresponding to each
     *                                         reading within provided fingerprint.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return ((PROMedSRobustLateration3DSolver) mLaterationSolver).
                getStopThreshold();
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        ((PROMedSRobustLateration3DSolver) mLaterationSolver).
                setStopThreshold(stopThreshold);
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Initializes robust lateration solver.
     */
    private void init() {
        mLaterationSolver = new PROMedSRobustLateration3DSolver(
                mTrilaterationSolverListener);
    }

    /**
     * Sets quality scores corresponding to each provided located radio source.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param sourceQualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetSourceQualityScores(final double[] sourceQualityScores) {
        if (sourceQualityScores == null ||
                sourceQualityScores.length < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mSourceQualityScores = sourceQualityScores;

        buildPositionsDistancesDistanceStandardDeviationsAndQualityScores();
    }

    /**
     * Sets quality scores corresponding to each provided reading within provided
     * fingerprint.
     * This method is used internally and does not check whether instance is locked
     * or not.
     *
     * @param fingerprintReadingsQualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores lengt is
     *                                  smaller than 3 samples.
     */
    private void internalSetFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) {
        if (fingerprintReadingsQualityScores == null ||
                fingerprintReadingsQualityScores.length < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mFingerprintReadingsQualityScores = fingerprintReadingsQualityScores;

        buildPositionsDistancesDistanceStandardDeviationsAndQualityScores();
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator.java 473
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator.java 1639
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator.java 1619
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPowerdBm() {
        return mInitialTransmittedPowerdBm;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     *
     * @param initialTransmittedPowerdBm initial transmitted power to start the
     *                                   estimation of radio source transmitted
     *                                   power.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialTransmittedPowerdBm(
            final Double initialTransmittedPowerdBm) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialTransmittedPowerdBm = initialTransmittedPowerdBm;
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPower() {
        return mInitialTransmittedPowerdBm != null ?
                Utils.dBmToPower(mInitialTransmittedPowerdBm) : null;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     *
     * @param initialTransmittedPower initial transmitted power to start the
     *                                estimation of radio source transmitted power.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setInitialTransmittedPower(
            final Double initialTransmittedPower) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (initialTransmittedPower != null) {
            if (initialTransmittedPower < 0.0) {
                throw new IllegalArgumentException();
            }
            mInitialTransmittedPowerdBm = Utils.powerTodBm(
                    initialTransmittedPower);
        } else {
            mInitialTransmittedPowerdBm = null;
        }
    }

    /**
     * Gets initial position to start the estimation of radio source position.
     * If not defined, centroid of provided fingerprints will be used.
     *
     * @return initial position to start the estimation of radio source position.
     */
    public P getInitialPosition() {
        return mInitialPosition;
    }

    /**
     * Sets initial position to start the estimation of radio source position.
     * If not defined, centroid of provided fingerprints will be used.
     *
     * @param initialPosition initial position to start the estimation of radio
     *                        source position.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialPosition(final P initialPosition) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialPosition = initialPosition;
    }

    /**
     * Gets initial exponent typically used on free space for path loss propagation
     * in terms of distance.
     * On different environments path loss exponent might have different value:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     * <p>
     * If path loss exponent estimation is enabled, estimation will start at this
     * value and will converge to the most appropriate value.
     * If path loss exponent estimation is disabled, this value will be assumed
     * to be exact and the estimated path loss exponent will be equal to this
     * value.
     *
     * @return initial path loss exponent.
     */
    public double getInitialPathLossExponent() {
        return mInitialPathLossExponent;
    }

    /**
     * Sets initial exponent typically used on free space for path loss propagation
     * in terms of distance.
     * On different environments path loss exponent might have different value:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     * <p>
     * If path loss exponent estimation is enabled, estimation will start at this
     * value and will converge to the most appropriate value.
     * If path loss exponent estimation is disabled, this value will be assumed
     * to be exact and the estimated path loss exponent will be equal to this
     * value.
     *
     * @param initialPathLossExponent initial path loss exponent.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialPathLossExponent(final double initialPathLossExponent)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialPathLossExponent = initialPathLossExponent;
    }

    /**
     * Indicates whether transmitted power estimation is enabled or not.
     *
     * @return true if transmitted power estimation is enabled, false otherwise.
     */
    public boolean isTransmittedPowerEstimationEnabled() {
        return mTransmittedPowerEstimationEnabled;
    }

    /**
     * Specifies whether transmitted power estimation is enabled or not.
     *
     * @param transmittedPowerEstimationEnabled true if transmitted power estimation is enabled,
     *                                          false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setTransmittedPowerEstimationEnabled(
            final boolean transmittedPowerEstimationEnabled)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mTransmittedPowerEstimationEnabled = transmittedPowerEstimationEnabled;
    }

    /**
     * Indicates whether radio source position estimation is enabled or not.
     *
     * @return true if position estimation is enabled, false otherwise.
     */
    public boolean isPositionEstimationEnabled() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 7131
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5398
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2621
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters for the general case.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the general purpose case.
     *               Must have length 9.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneral(final double[] params) throws EvaluationException {
        final double m11 = params[0];
        final double m21 = params[1];
        final double m31 = params[2];

        final double m12 = params[3];
        final double m22 = params[4];
        final double m32 = params[5];

        final double m13 = params[6];
        final double m23 = params[7];
        final double m33 = params[8];

        return evaluate(m11, m21, m31, m12, m22, m32,
                m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters when common z-axis is assumed.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the common z-axis case.
     *               Must have length 6.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateCommonAxis(final double[] params) throws EvaluationException {
        final double m11 = params[0];

        final double m12 = params[1];
        final double m22 = params[2];

        final double m13 = params[3];
        final double m23 = params[4];
        final double m33 = params[5];

        return evaluate(m11, 0.0, 0.0, m12, m22, 0.0,
                m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param m11 element 1,1 of cross-coupling error matrix.
     * @param m21 element 2,1 of cross-coupling error matrix.
     * @param m31 element 3,1 of cross-coupling error matrix.
     * @param m12 element 1,2 of cross-coupling error matrix.
     * @param m22 element 2,2 of cross-coupling error matrix.
     * @param m32 element 3,2 of cross-coupling error matrix.
     * @param m13 element 1,3 of cross-coupling error matrix.
     * @param m23 element 2,3 of cross-coupling error matrix.
     * @param m33 element 3,3 of cross-coupling error matrix.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluate(final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33)
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1089
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3176
            mRunning = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2728
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 612
            mRunning = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7710
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3652
                        return evaluateCommonAxis(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];

        final double m12 = result[4];
        final double m22 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
        b.setElementAtIndex(0, bx);
        b.setElementAtIndex(1, by);
        b.setElementAtIndex(2, bz);

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, 0.0);
        m.setElementAtIndex(2, 0.0);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        setResult(m, b);
    }

    /**
     * Makes proper conversion of internal cross-coupling and bias matrices.
     *
     * @param m internal cross-coupling matrix.
     * @param b internal bias matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m, final Matrix b) throws AlgebraException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2798
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4235
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3154
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1748
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4560
            mRunning = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5646
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6968
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5922
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7309
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 706
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3704
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3844
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1798
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3010
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2800
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4237
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2119
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4470
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8597
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10011
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9035
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10531
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3054
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2844
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4281
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2994
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4388
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4514
    public Matrix getEstimatedMg() {
        return mEstimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMg != null ?
                mEstimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    public Matrix getEstimatedGg() {
        return mEstimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2010
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2026
            final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    public int getMinimumRequiredMeasurements() {
        return mCommonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS :
                MINIMUM_MEASUREMENTS_GENERAL;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= getMinimumRequiredMeasurements()
                && mPosition != null && mYear != null;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Gets Earth's magnetic model.
     *
     * @return Earth's magnetic model or null if not provided.
     */
    public WorldMagneticModel getMagneticModel() {
        return mMagneticModel;
    }

    /**
     * Sets Earth's magnetic model.
     *
     * @param magneticModel Earth's magnetic model to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMagneticModel(final WorldMagneticModel magneticModel)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMagneticModel = magneticModel;
    }

    /**
     * Estimates magnetometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException
                | com.irurueta.numerical.NotReadyException
                | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    public Matrix getEstimatedMm() {
File Line
com/irurueta/navigation/indoor/position/RobustRangingAndRssiPositionEstimator2D.java 969
com/irurueta/navigation/indoor/position/RobustRangingPositionEstimator2D.java 977
com/irurueta/navigation/indoor/position/RobustRssiPositionEstimator2D.java 969
            final RobustRangingAndRssiPositionEstimatorListener<Point2D> listener) {
        return create(sourceQualityScores, fingerprintReadingQualityScores,
                sources, fingerprint, listener, DEFAULT_ROBUST_METHOD);
    }

    /**
     * Sets positions, distances and standard deviations of distances on internal
     * lateration solver.
     *
     * @param positions                  positions to be set.
     * @param distances                  distances to be set.
     * @param distanceStandardDeviations standard deviations of distances to be set.
     * @param distanceQualityScores      distance quality scores or null if not
     *                                   required.
     */
    @Override
    protected void setPositionsDistancesDistanceStandardDeviationsAndQualityScores(
            final List<Point2D> positions, List<Double> distances,
            final List<Double> distanceStandardDeviations,
            final List<Double> distanceQualityScores) {
        final int size = positions.size();
        Point2D[] positionsArray = new InhomogeneousPoint2D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];

        double[] qualityScoresArray = null;
        if (distanceQualityScores != null) {
            qualityScoresArray = new double[size];
        }

        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);

            if (qualityScoresArray != null) {
                qualityScoresArray[i] = distanceQualityScores.get(i);
            }
        }

        try {
            mLaterationSolver.setPositionsDistancesAndStandardDeviations(
                    positionsArray, distancesArray, distanceStandardDeviationsArray);

            if (qualityScoresArray != null) {
                mLaterationSolver.setQualityScores(qualityScoresArray);
            }
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }
}
File Line
com/irurueta/navigation/indoor/position/RobustRangingAndRssiPositionEstimator3D.java 969
com/irurueta/navigation/indoor/position/RobustRssiPositionEstimator3D.java 969
            final RobustRangingAndRssiPositionEstimatorListener<Point3D> listener) {
        return create(sourceQualityScores, fingerprintReadingQualityScores,
                sources, fingerprint, listener, DEFAULT_ROBUST_METHOD);
    }

    /**
     * Sets positions, distances and standard deviations of distances on internal
     * lateration solver.
     *
     * @param positions                  positions to be set.
     * @param distances                  distances to be set.
     * @param distanceStandardDeviations standard deviations of distances to be set.
     * @param distanceQualityScores      distance quality scores or null if not
     *                                   required.
     */
    @Override
    protected void setPositionsDistancesDistanceStandardDeviationsAndQualityScores(
            final List<Point3D> positions, List<Double> distances,
            final List<Double> distanceStandardDeviations,
            final List<Double> distanceQualityScores) {
        final int size = positions.size();
        Point3D[] positionsArray = new InhomogeneousPoint3D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];

        double[] qualityScoresArray = null;
        if (distanceQualityScores != null) {
            qualityScoresArray = new double[size];
        }

        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);

            if (qualityScoresArray != null) {
                qualityScoresArray[i] = distanceQualityScores.get(i);
            }
        }

        try {
            mLaterationSolver.setPositionsDistancesAndStandardDeviations(
                    positionsArray, distancesArray, distanceStandardDeviationsArray);

            if (qualityScoresArray != null) {
                mLaterationSolver.setQualityScores(qualityScoresArray);
            }
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4213
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3914
            final double bx, final double by, final double bz,
            final double m11, final double m21, final double m31,
            final double m12, final double m22, final double m32,
            final double m13, final double m23, final double m33,
            final double g11, final double g21, final double g31,
            final double g12, final double g22, final double g32,
            final double g13, final double g23, final double g33)
            throws EvaluationException {

        try {
            final BodyKinematicsSequence<StandardDeviationTimedBodyKinematics> measuredSequence =
                    mSequences.get(i);
            final BodyKinematicsSequence<StandardDeviationTimedBodyKinematics> fixedSequence =
                    mFixedSequences.get(i);

            // generate new sequence using current parameters to fix angular rate measurements
            if (mMeasuredSpecificForce == null) {
                mMeasuredSpecificForce = new Matrix(
                        BodyKinematics.COMPONENTS, 1);
            }
            if (mTrueSpecificForce == null) {
                mTrueSpecificForce = new Matrix(
                        BodyKinematics.COMPONENTS, 1);
            }

            if (mMeasuredAngularRate == null) {
                mMeasuredAngularRate = new Matrix(
                        BodyKinematics.COMPONENTS, 1);
            }
            if (mTrueAngularRate == null) {
                mTrueAngularRate = new Matrix(
                        BodyKinematics.COMPONENTS, 1);
            }

            if (mM == null) {
                mM = new Matrix(BodyKinematics.COMPONENTS,
                        BodyKinematics.COMPONENTS);
            }
            if (mInvM == null) {
                mInvM = new Matrix(BodyKinematics.COMPONENTS,
                        BodyKinematics.COMPONENTS);
            }
            if (mB == null) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5841
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7158
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8296
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9704
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6124
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7516
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8710
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10200
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2171
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2216
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1197
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1242
    public static void estimateKinematics(final double timeInterval,
                                          final CoordinateTransformation c,
                                          final CoordinateTransformation oldC,
                                          final Speed vx, final Speed vy, final Speed vz,
                                          final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                          final double x, final double y, final double z,
                                          final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC,
                SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 6254
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 6043
            mEstimatedMg = preliminaryResult.mEstimatedMg;
            mEstimatedGg = preliminaryResult.mEstimatedGg;
        }
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(),
                acceleration.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second.
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value.
     */
    private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
        return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(),
                angularSpeed.getUnit(), AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Fixes a measured kinematics instance using current
     *
     * @param measuredKinematics a measured kinematics instance.
     * @param result             instance where fixed values will be stored.
     * @throws AlgebraException if accelerometer or gyroscope parameters
     *                          contain numerical instabilities.
     */
    private void fixKinematics(
            final BodyKinematics measuredKinematics,
            final BodyKinematics result) throws AlgebraException {

        mMeasuredSpecificForce[0] = measuredKinematics.getFx();
        mMeasuredSpecificForce[1] = measuredKinematics.getFy();
        mMeasuredSpecificForce[2] = measuredKinematics.getFz();
        mAccelerationFixer.fix(mMeasuredSpecificForce,
                mFixedSpecificForce);

        mMeasuredAngularRate[0] = measuredKinematics.getAngularRateX();
        mMeasuredAngularRate[1] = measuredKinematics.getAngularRateY();
        mMeasuredAngularRate[2] = measuredKinematics.getAngularRateZ();
        mAngularRateFixer.fix(mMeasuredAngularRate,
                mFixedSpecificForce, mFixedAngularRate);

        result.setSpecificForceCoordinates(
                mFixedSpecificForce[0],
                mFixedSpecificForce[1],
                mFixedSpecificForce[2]);
        result.setAngularRateCoordinates(
                mFixedAngularRate[0],
                mFixedAngularRate[1],
                mFixedAngularRate[2]);
    }

    /**
     * Internal class containing estimated preliminary result.
     */
    protected static class PreliminaryResult {
        /**
         * Estimated gyroscope biases for each IMU axis expressed in radians per second
         * (rad/s).
         */
        private double[] mEstimatedBiases;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6614
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1693
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2364
            mRunning = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 168
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 175
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 156
            final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<PreliminaryResult> innerEstimator =
                new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustKnownFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 640
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 625
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<PreliminaryResult> innerEstimator =
                new MSACRobustEstimator<>(
                        new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation,
                            final int i) {
                        return computeError(mSequences.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustEasyGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1835
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1865
                commonAxisUsed, estimateGDependentCrossBiases, bias,
                initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<PreliminaryResult> innerEstimator =
                new MSACRobustEstimator<>(
                        new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustKnownBiasTurntableGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8021
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8204
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8496
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9910
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must be
     *                                      3x1 and is expressed in radians
     *                                      per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9429
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9612
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8419
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8612
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8925
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10421
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.*
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9907
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10102
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2955
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1621
                                                                final ECEFFrame frame, final ECEFFrame oldFrame) {
        final BodyKinematics result = new BodyKinematics();
        estimateKinematics(timeInterval, frame, oldFrame, result);
        return result;
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static BodyKinematics estimateKinematicsAndReturnNew(final double timeInterval,
                                                                final CoordinateTransformation c,
                                                                final CoordinateTransformation oldC,
                                                                final Speed vx, final Speed vy, final Speed vz,
                                                                final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                                                final double x, final double y, final double z) {
        final BodyKinematics result = new BodyKinematics();
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                x, y, z, result);
        return result;
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static BodyKinematics estimateKinematicsAndReturnNew(final Time timeInterval,
                                                                final CoordinateTransformation c,
                                                                final CoordinateTransformation oldC,
                                                                final Speed vx, final Speed vy, final Speed vz,
                                                                final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                                                final double x, final double y, final double z) {
        final BodyKinematics result = new BodyKinematics();
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                x, y, z, result);
        return result;
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param position     cartesian body position resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static BodyKinematics estimateKinematicsAndReturnNew(final double timeInterval,
                                                                final CoordinateTransformation c,
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator.java 820
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator.java 793
        final RangingAndRssiReadingLocated<S, P> reading = mReadings.get(i);
        final double frequency = reading.getSource().getFrequency();

        final double pathLossExponent = currentEstimation.getEstimatedPathLossExponent();

        //compute k as the constant part of the isotropic received power formula
        //so that: Pr = Pte*k^n/d^n
        final double k = RssiRadioSourceEstimator.SPEED_OF_LIGHT /
                (4.0 * Math.PI * frequency);
        final double kdB = 10.0 * pathLossExponent * Math.log10(k);

        //get distance from estimated radio source position and reading position
        final P readingPosition = reading.getPosition();
        final P radioSourcePosition = currentEstimation.getEstimatedPosition();

        final double sqrDistance = radioSourcePosition.sqrDistanceTo(readingPosition);

        final double transmittedPowerdBm = currentEstimation.
                getEstimatedTransmittedPowerdBm();

        //compute expected received power assuming isotropic transmission
        //and compare agains measured RSSI at fingerprint location
        final double expectedRSSI = kdB + transmittedPowerdBm -
                5.0 * pathLossExponent * Math.log10(sqrDistance);
        final double rssi = reading.getRssi();

        return Math.abs(expectedRSSI - rssi);
    }

    /**
     * Contains a solution obtained during robust estimation for a subset of
     * samples.
     *
     * @param <P> a {@link Point} type.
     */
    static class Solution<P extends Point<?>> {
        /**
         * Estimated position for a subset of samples.
         */
        private final P mEstimatedPosition;

        /**
         * Estimated transmitted power expressed in dBm's for a subset of samples.
         */
        private final double mEstimatedTransmittedPowerdBm;

        /**
         * Estimated path loss exponent for a subset of samples.
         */
        private final double mEstimatedPathLossExponent;

        /**
         * Constructor.
         *
         * @param estimatedPosition            estimated position for a subset of samples.
         * @param estimatedTransmittedPowerdBm estimated transmitted power expressed
         *                                     in dBm's for a subset of samples.
         * @param estimatedPathLossExponent    estimated path loss exponent.
         */
        public Solution(final P estimatedPosition,
                        final double estimatedTransmittedPowerdBm,
                        final double estimatedPathLossExponent) {
            mEstimatedPosition = estimatedPosition;
            mEstimatedTransmittedPowerdBm = estimatedTransmittedPowerdBm;
            mEstimatedPathLossExponent = estimatedPathLossExponent;
        }

        /**
         * Gets estimated position for a subset of samples.
         *
         * @return estimated position for a subset of samples.
         */
        public P getEstimatedPosition() {
            return mEstimatedPosition;
        }

        /**
         * Gets estimated transmitted power expressed in dBm's for a subset of
         * samples.
         *
         * @return estimated transmitted power expressed in dBm's for a subset
         * of samples.
         */
        public double getEstimatedTransmittedPowerdBm() {
            return mEstimatedTransmittedPowerdBm;
        }

        /**
         * Gets estimated path loss exponent.
         *
         * @return estimated path loss exponent.
         */
        public double getEstimatedPathLossExponent() {
            return mEstimatedPathLossExponent;
        }
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 790
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 797
        super(position, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        mGravityNorm = computeGravityNorm();

        final MSACRobustEstimator<PreliminaryResult> innerEstimator =
                new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6616
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7223
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2050
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2161
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
        return mEstimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMa != null ?
                mEstimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2113
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2221
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1536
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1419
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2320
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2428
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    public Matrix getEstimatedMm() {
        return mEstimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    public Double getEstimatedSx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    public Double getEstimatedSy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    public Double getEstimatedSz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMxy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMxz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMyx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    public Double getEstimatedMyz() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    public Double getEstimatedMzx() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    public Double getEstimatedMzy() {
        return mEstimatedMm != null ?
                mEstimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     *
     * @return estimated covariance matrix for estimated position.
     */
    public Matrix getEstimatedCovariance() {
        return mEstimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
File Line
com/irurueta/navigation/indoor/position/NonLinearMixedPositionEstimator2D.java 288
com/irurueta/navigation/indoor/position/NonLinearRangingAndRssiPositionEstimator2D.java 288
com/irurueta/navigation/indoor/position/NonLinearRangingPositionEstimator2D.java 291
            final MixedPositionEstimatorListener<Point2D> listener) {
        super(initialPosition, listener);
        init();
        internalSetSources(sources);
        internalSetFingerprint(fingerprint);
    }

    /**
     * Gets estimated position.
     *
     * @return estimated position.
     */
    public Point2D getEstimatedPosition() {
        if (mEstimatedPositionCoordinates == null) {
            return null;
        }

        final InhomogeneousPoint2D result = new InhomogeneousPoint2D();
        getEstimatedPosition(result);
        return result;
    }

    /**
     * Sets positions, distances and standard deviations of distances on internal
     * lateration solver.
     *
     * @param positions                  positions to be set.
     * @param distances                  distances to be set.
     * @param distanceStandardDeviations standard deviations of distances to be set.
     */
    @Override
    @SuppressWarnings("Duplicates")
    protected void setPositionsDistancesAndDistanceStandardDeviations(
            final List<Point2D> positions, final List<Double> distances,
            final List<Double> distanceStandardDeviations) {

        final int size = positions.size();
        Point2D[] positionsArray = new InhomogeneousPoint2D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];
        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);
        }

        try {
            mTrilaterationSolver.setPositionsDistancesAndStandardDeviations(
                    positionsArray, distancesArray, distanceStandardDeviationsArray);
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }

    /**
     * Initializes lateration solver.
     */
    private void init() {
        mTrilaterationSolver = new NonLinearLeastSquaresLateration2DSolver(
                mLaterationSolverListener);
    }
}
File Line
com/irurueta/navigation/indoor/position/NonLinearRangingAndRssiPositionEstimator3D.java 291
com/irurueta/navigation/indoor/position/NonLinearRangingPositionEstimator3D.java 291
            final RangingAndRssiPositionEstimatorListener<Point3D> listener) {
        super(initialPosition, listener);
        init();
        internalSetSources(sources);
        internalSetFingerprint(fingerprint);
    }

    /**
     * Gets estimated position.
     *
     * @return estimated position.
     */
    public Point3D getEstimatedPosition() {
        if (mEstimatedPositionCoordinates == null) {
            return null;
        }

        final InhomogeneousPoint3D result = new InhomogeneousPoint3D();
        getEstimatedPosition(result);
        return result;
    }

    /**
     * Sets positions, distances and standard deviations of distances on internal
     * lateration solver.
     *
     * @param positions                  positions to be set.
     * @param distances                  distances to be set.
     * @param distanceStandardDeviations standard deviations of distances to be set.
     */
    @Override
    @SuppressWarnings("Duplicates")
    protected void setPositionsDistancesAndDistanceStandardDeviations(
            final List<Point3D> positions, final List<Double> distances,
            final List<Double> distanceStandardDeviations) {

        final int size = positions.size();
        Point3D[] positionsArray = new InhomogeneousPoint3D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];
        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);
        }

        try {
            mTrilaterationSolver.setPositionsDistancesAndStandardDeviations(
                    positionsArray, distancesArray, distanceStandardDeviationsArray);
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }

    /**
     * Initializes lateration solver.
     */
    private void init() {
        mTrilaterationSolver = new NonLinearLeastSquaresLateration3DSolver(
                mLaterationSolverListener);
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1558
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1528
                                    PROSACRobustEasyGyroscopeCalibrator.this, progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            setupAccelerationFixer();

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < EasyGyroscopeCalibrator.MINIMUM_SEQUENCES_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/lateration/HomogeneousLinearLeastSquaresLateration2DSolver.java 93
com/irurueta/navigation/lateration/InhomogeneousLinearLeastSquaresLateration2DSolver.java 93
    public HomogeneousLinearLeastSquaresLateration2DSolver(
            final Circle[] circles,
            final LaterationSolverListener<Point2D> listener) {
        super(listener);
        internalSetCircles(circles);
    }

    /**
     * Gets circles defined by provided positions and distances.
     *
     * @return circles defined by provided positions and distances.
     */
    public Circle[] getCircles() {
        if (mPositions == null) {
            return null;
        }

        final Circle[] result = new Circle[mPositions.length];

        for (int i = 0; i < mPositions.length; i++) {
            result[i] = new Circle(mPositions[i], mDistances[i]);
        }
        return result;
    }

    /**
     * Sets circles defining positions and euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 2.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setCircles(final Circle[] circles) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetCircles(circles);
    }

    /**
     * Gets number of dimensions of provided points.
     *
     * @return always returns 2 dimensions.
     */
    @Override
    public int getNumberOfDimensions() {
        return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH;
    }

    /**
     * Minimum required number of positions and distances.
     * At least 3 positions and distances will be required to linearly solve a 2D problem.
     *
     * @return minimum required number of positions and distances.
     */
    @Override
    public int getMinRequiredPositionsAndDistances() {
        return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
    }

    /**
     * Gets estimated position.
     *
     * @return estimated position.
     */
    @Override
    public Point2D getEstimatedPosition() {
        if (mEstimatedPositionCoordinates == null) {
            return null;
        }

        final InhomogeneousPoint2D position = new InhomogeneousPoint2D();
        getEstimatedPosition(position);
        return position;
    }

    /**
     * Internally sets circles defining positions and euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 3.
     */
    private void internalSetCircles(final Circle[] circles) {
        if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 659
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1884
        super(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 644
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1854
        super(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, bias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3648
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3605
            final KnownFrameMagnetometerNonLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Indicates whether calibrator is ready to start the estimator.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or no.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Gets Earth's magnetic model.
     *
     * @return Earth's magnetic model or null if not provided.
     */
    @Override
    public WorldMagneticModel getMagneticModel() {
        return mMagneticModel;
    }

    /**
     * Sets Earth's magnetic model.
     *
     * @param magneticModel Earth's magnetic model to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMagneticModel(final WorldMagneticModel magneticModel)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMagneticModel = magneticModel;
    }

    /**
     * Estimates magnetometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException |
                com.irurueta.numerical.NotReadyException |
                IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    @Override
    public double[] getEstimatedHardIron() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2752
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2924
            mInvM.multiply(mBm, mB);

            mInvM.multiply(mBmeas, mBtrue);
            mBtrue.subtract(mB);

            final double norm = Utils.normF(mBtrue);
            return norm * norm;

        } catch (final AlgebraException e) {
            throw new EvaluationException(e);
        }
    }

    /**
     * Converts a time instance expressed in milliseconds since epoch time
     * (January 1st, 1970 at midnight) to a decimal year.
     *
     * @param timestampMillis milliseconds value to be converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final Long timestampMillis) {
        if (timestampMillis == null) {
            return null;
        }

        final GregorianCalendar calendar = new GregorianCalendar();
        calendar.setTimeInMillis(timestampMillis);
        return convertTime(calendar);
    }

    /**
     * Converts a time instant contained ina date object to a
     * decimal year.
     *
     * @param date a time instance to be converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final Date date) {
        if (date == null) {
            return null;
        }

        final GregorianCalendar calendar = new GregorianCalendar();
        calendar.setTime(date);
        return convertTime(calendar);
    }

    /**
     * Converts a time instant contained in a gregorian calendar to a
     * decimal year.
     *
     * @param calendar calendar containing a specific instant to be
     *                 converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final GregorianCalendar calendar) {
        if (calendar == null) {
            return null;
        }

        return WMMEarthMagneticFluxDensityEstimator.convertTime(calendar);
    }

    /**
     * Converts provided ECEF position to position expressed in NED
     * coordinates.
     *
     * @param position ECEF position to be converted.
     * @return converted position expressed in NED coordinates.
     */
    private static NEDPosition convertPosition(final ECEFPosition position) {
        final NEDVelocity velocity = new NEDVelocity();
        final NEDPosition result = new NEDPosition();
        ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                position.getX(), position.getY(), position.getZ(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator.java 521
com/irurueta/navigation/indoor/radiosource/RangingAndRssiRadioSourceEstimator.java 516
com/irurueta/navigation/indoor/radiosource/RssiRadioSourceEstimator.java 518
            final MixedRadioSourceEstimatorListener<S, P> listener) {
        this(readings, initialPosition, initialTransmittedPowerdBm, listener);
        mInitialPathLossExponent = initialPathLossExponent;
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPowerdBm() {
        return mInitialTransmittedPowerdBm;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @param initialTransmittedPowerdBm initial transmitted power to start the
     *                                   estimation of radio source transmitted
     *                                   power.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialTransmittedPowerdBm(final Double initialTransmittedPowerdBm)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialTransmittedPowerdBm = initialTransmittedPowerdBm;
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPower() {
        return mInitialTransmittedPowerdBm != null ?
                Utils.dBmToPower(mInitialTransmittedPowerdBm) : null;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @param initialTransmittedPower initial transmitted power to start the
     *                                estimation of radio source transmitted power.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setInitialTransmittedPower(final Double initialTransmittedPower)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (initialTransmittedPower != null) {
            if (initialTransmittedPower < 0.0) {
                throw new IllegalArgumentException();
            }
            mInitialTransmittedPowerdBm = Utils.powerTodBm(
                    initialTransmittedPower);
        } else {
            mInitialTransmittedPowerdBm = null;
        }
    }

    /**
     * Indicates whether transmitted power estimation is enabled or not.
     *
     * @return true if transmitted power estimation is enabled, false otherwise.
     */
    public boolean isTransmittedPowerEstimationEnabled() {
        return mTransmittedPowerEstimationEnabled;
    }

    /**
     * Specifies whether transmitted power estimation is enabled or not.
     *
     * @param transmittedPowerEstimationEnabled true if transmitted power estimation is enabled,
     *                                          false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setTransmittedPowerEstimationEnabled(
            final boolean transmittedPowerEstimationEnabled)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mTransmittedPowerEstimationEnabled = transmittedPowerEstimationEnabled;
    }

    /**
     * Gets initial position to start the estimation of radio source position.
     * If not defined, centroid of provided readings will be used.
     * <p>
     * If position estimation is enabled, estimation will start at this value
     * and will converge to the most appropriate value.
     * If position estimation is disabled, this value will be assumed to
     * be exact and the estimated position will be equal to this value.
     *
     * @return initial position to start the estimation of radio source position.
     */
    public P getInitialPosition() {
        return mInitialPosition;
    }

    /**
     * Sets initial position to start the estimation of radio source position.
     * If not defined, centroid of provided fingerprints will be used.
     * <p>
     * If position estimation is enabled, estimation will start at this value
     * and will converge to the most appropriate value.
     * If position estimation is disabled, this value will be assumed to
     * be exact and the estimated position will be equal to this value.
     *
     * @param initialPosition initial position to start the estimation of radio
     *                        source position.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialPosition(final P initialPosition) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialPosition = initialPosition;
    }

    /**
     * Gets initial exponent typically used on free space for path loss propagation
     * in terms of distance.
     * On different environments path loss exponent might have different value:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     * <p>
     * If path loss exponent estimation is enabled, estimation will start at this
     * value and will converge to the most appropriate value.
     * If path loss exponent estimation is disabled, this value will be assumed
     * to be exact and the estimated path loss exponent will be equal to this
     * value.
     *
     * @return initial path loss exponent.
     */
    public double getInitialPathLossExponent() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 169
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 713
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 176
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 157
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<PreliminaryResult> innerEstimator =
                new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustKnownFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5552
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6874
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8606
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9816
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5821
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7203
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9044
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10321
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 708
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 933
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<Matrix> innerEstimator =
                new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Matrix>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<Matrix> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final Matrix currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 169
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 713
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1836
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 176
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1866
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 157
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 934
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<PreliminaryResult> innerEstimator =
                new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustKnownFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4266
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5891
            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);

            mB.setElementAtIndex(0, bx);
            mB.setElementAtIndex(1, by);
            mB.setElementAtIndex(2, bz);

            mG.setElementAt(0, 0, g11);
            mG.setElementAt(1, 0, g21);
            mG.setElementAt(2, 0, g31);

            mG.setElementAt(0, 1, g12);
            mG.setElementAt(1, 1, g22);
            mG.setElementAt(2, 1, g32);

            mG.setElementAt(0, 2, g13);
            mG.setElementAt(1, 2, g23);
            mG.setElementAt(2, 2, g33);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1889
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1912
                                    PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.this, progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum required
     *                                  number of samples (10 or 13).
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinimumRequiredMeasurements()) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/indoor/position/RobustMixedPositionEstimator2D.java 973
com/irurueta/navigation/indoor/position/RobustRangingAndRssiPositionEstimator2D.java 969
com/irurueta/navigation/indoor/position/RobustRangingPositionEstimator2D.java 977
com/irurueta/navigation/indoor/position/RobustRssiPositionEstimator2D.java 969
            final RobustMixedPositionEstimatorListener<Point2D> listener) {
        return create(sourceQualityScores, fingerprintReadingQualityScores,
                sources, fingerprint, listener, DEFAULT_ROBUST_METHOD);
    }

    /**
     * Sets positions, distances and standard deviations of distances on internal
     * lateration solver.
     *
     * @param positions                  positions to be set.
     * @param distances                  distances to be set.
     * @param distanceStandardDeviations standard deviations of distances to be set.
     * @param distanceQualityScores      distance quality scores or null if not
     *                                   required.
     */
    @Override
    protected void setPositionsDistancesDistanceStandardDeviationsAndQualityScores(
            final List<Point2D> positions, List<Double> distances,
            final List<Double> distanceStandardDeviations,
            final List<Double> distanceQualityScores) {
        final int size = positions.size();
        Point2D[] positionsArray = new InhomogeneousPoint2D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];

        double[] qualityScoresArray = null;
        if (distanceQualityScores != null) {
            qualityScoresArray = new double[size];
        }

        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);

            if (qualityScoresArray != null) {
                qualityScoresArray[i] = distanceQualityScores.get(i);
            }
        }

        try {
            mLaterationSolver.setPositionsDistancesAndStandardDeviations(
                    positionsArray, distancesArray, distanceStandardDeviationsArray);

            if (qualityScoresArray != null) {
                mLaterationSolver.setQualityScores(qualityScoresArray);
            }
        } catch (LockedException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5295
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5466
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5748
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7065
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must be
     *                                      3x1 and is expressed in radians
     *                                      per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6617
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6788
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7664
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9069
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5546
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5729
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6020
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7412
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6928
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7111
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8047
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9534
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/lateration/PROSACRobustLateration2DSolver.java 407
com/irurueta/navigation/lateration/PROSACRobustLateration3DSolver.java 406
        super(circles, distanceStandardDeviations, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mDistances.length;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Solves the lateration problem.
     *
     * @return estimated position.
     * @throws LockedException          if instance is busy solving the lateration problem.
     * @throws NotReadyException        is solver is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public Point2D solve() throws LockedException, NotReadyException,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 561
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1791
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 568
                                    PROSACRobustKnownFrameAccelerometerCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 664
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 649
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(
                        new LMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation,
                            final int i) {
                        return computeError(mSequences.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return LMedSRobustEasyGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1859
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 1890
                commonAxisUsed, estimateGDependentCrossBiases, bias,
                initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(
                        new LMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return LMedSRobustKnownBiasTurntableGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 199
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 180
            final RobustKnownFrameGyroscopeCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return LMedSRobustKnownFrameGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 660
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1855
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 645
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1885
                estimateGDependentCrossBiases, bias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
File Line
com/irurueta/navigation/lateration/RobustLateration2DSolver.java 1317
com/irurueta/navigation/lateration/RobustLateration3DSolver.java 1319
            final Point2D[] inlierPositions = new Point2D[nInliers];
            final double[] inlierDistances = new double[nInliers];
            double[] inlierStandardDeviations = null;
            if (mDistanceStandardDeviations != null) {
                inlierStandardDeviations = new double[nInliers];
            }
            int pos = 0;
            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    //sample is inlier
                    inlierPositions[pos] = mPositions[i];
                    inlierDistances[pos] = mDistances[i];
                    if (inlierStandardDeviations != null) {
                        inlierStandardDeviations[pos] = mDistanceStandardDeviations[i];
                    }
                    pos++;
                }
            }

            try {
                mNonLinearSolver.setInitialPosition(position);
                if (inlierStandardDeviations != null) {
                    mNonLinearSolver.setPositionsDistancesAndStandardDeviations(
                            inlierPositions, inlierDistances, inlierStandardDeviations);
                } else {
                    mNonLinearSolver.setPositionsAndDistances(
                            inlierPositions, inlierDistances);
                }
                mNonLinearSolver.solve();

                if (mKeepCovariance) {
                    //keep covariance
                    mCovariance = mNonLinearSolver.getCovariance();
                } else {
                    mCovariance = null;
                }

                mEstimatedPosition = mNonLinearSolver.getEstimatedPosition();
            } catch (Exception e) {
                //refinement failed, so we return input value
                mCovariance = null;
                mEstimatedPosition = position;
            }
        } else {
            mCovariance = null;
            mEstimatedPosition = position;
        }

        return mEstimatedPosition;
    }

    /**
     * Solves a preliminar solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminar solution will be stored.
     */
    protected void solvePreliminarSolutions(
            final int[] samplesIndices, final List<Point2D> solutions) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 810
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 820
        super(position, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        mGravityNorm = computeGravityNorm();

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8198
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9606
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8305
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8505
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9713
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9919
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8606
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10096
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8719
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8934
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10209
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10430
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 818
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 927
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double fMeasX = measuredKinematics.getFx();
            final double fMeasY = measuredKinematics.getFy();
            final double fMeasZ = measuredKinematics.getFz();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, 1.0);
            a.setElementAt(i, 3, fTrueX);
            a.setElementAt(i, 6, fTrueY);
            a.setElementAt(i, 7, fTrueZ);

            b.setElementAtIndex(i, fMeasX - fTrueX);
            i++;

            a.setElementAt(i, 1, 1.0);
            a.setElementAt(i, 4, fTrueY);
            a.setElementAt(i, 8, fTrueZ);
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 1544
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 818
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, velocity, oldVelocity, position);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(final double timeInterval,
                                               final CoordinateTransformation c,
                                               final CoordinateTransformation oldC,
                                               final Speed vx, final Speed vy, final Speed vz,
                                               final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                               final Point3D position) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz,
                oldVx, oldVy, oldVz, position);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(final Time timeInterval,
                                               final CoordinateTransformation c,
                                               final CoordinateTransformation oldC,
                                               final Speed vx, final Speed vy, final Speed vz,
                                               final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                               final Point3D position) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz,
                oldVx, oldVy, oldVz, position);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates)..
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF-frame coordinate transformation.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
     *                     along ECEF-frame axes.
     * @param vy           y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
     *                     along ECEF-frame axes.
     * @param vz           z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
     *                     along ECEF-frame axes.
     * @param oldVx        x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
     *                     resolved along ECEF-frame axes.
     * @param oldVy        y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
     *                     resolved along ECEF-frame axes.
     * @param oldVz        z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
     *                     resolved along ECEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m) and resolved along
     *                     ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m) and resolved along
     *                     ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m) and resolved along
     *                     ECEF-frame axes.
     * @param result       instance where estimated body kinematics will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final double timeInterval,
                                          final CoordinateTransformation c,
                                          final CoordinateTransformation oldC,
                                          final double vx, final double vy, final double vz,
                                          final double oldVx, final double oldVy, final double oldVz,
                                          final double x, final double y, final double z,
                                          final BodyKinematics result) {
        if (timeInterval < 0.0
                || !ECEFFrame.isValidCoordinateTransformation(c)
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/AccelerometerNonLinearCalibrator.java 25
com/irurueta/navigation/inertial/calibration/gyroscope/GyroscopeNonLinearCalibrator.java 24
com/irurueta/navigation/inertial/calibration/magnetometer/MagnetometerNonLinearCalibrator.java 24
        extends AccelerometerCalibrator {

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    double getInitialSx();

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialSx(final double initialSx) throws LockedException;

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    double getInitialSy();

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialSy(final double initialSy) throws LockedException;

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    double getInitialSz();

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialSz(final double initialSz) throws LockedException;

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    double getInitialMxy();

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialMxy(final double initialMxy) throws LockedException;

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    double getInitialMxz();

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialMxz(final double initialMxz) throws LockedException;

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    double getInitialMyx();

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialMyx(final double initialMyx) throws LockedException;

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    double getInitialMyz();

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialMyz(final double initialMyz) throws LockedException;

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    double getInitialMzx();

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialMzx(final double initialMzx) throws LockedException;

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    double getInitialMzy();

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialMzy(final double initialMzy) throws LockedException;

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz)
            throws LockedException;

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException;

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException;

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    Matrix getInitialMa();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6922
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5041
                        mFmeasZ = point[2];

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateGeneral(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double m11 = result[0];
        final double m21 = result[1];
        final double m31 = result[2];

        final double m12 = result[3];
        final double m22 = result[4];
        final double m32 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, m21);
        m.setElementAtIndex(2, m31);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, m32);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        setResult(m);
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws FittingException                         if Levenberg-Marquardt fails for numerical reasons.
     * @throws AlgebraException                         if there are numerical instabilities that prevent
     *                                                  matrix inversion.
     * @throws com.irurueta.numerical.NotReadyException never happens.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1538
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1506
                                    PROMedSRobustEasyGyroscopeCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            setupAccelerationFixer();

            mInliersData = null;
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < TurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7835
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9243
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8015
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9423
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8224
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9711
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8413
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9901
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 648
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 338
                         final ECEFFrame frame,
                         final CoordinateTransformation oldC,
                         final Speed oldVx, final Speed oldVy, final Speed oldVz,
                         final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(final double timeInterval,
                         final CoordinateTransformation c,
                         final CoordinateTransformation oldC,
                         final double vx, final double vy, final double vz,
                         final double oldVx, final double oldVy, final double oldVz,
                         final Point3D position, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                position, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(final Time timeInterval,
                         final CoordinateTransformation c,
                         final CoordinateTransformation oldC,
                         final double vx, final double vy, final double vz,
                         final double oldVx, final double oldVy, final double oldVz,
                         final Point3D position, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                position, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(final double timeInterval,
                         final CoordinateTransformation c,
                         final CoordinateTransformation oldC,
                         final ECEFVelocity velocity,
File Line
com/irurueta/navigation/indoor/position/SequentialRobustMixedPositionEstimator.java 41
com/irurueta/navigation/indoor/position/SequentialRobustRangingAndRssiPositionEstimator.java 38
public abstract class SequentialRobustMixedPositionEstimator<P extends Point<?>> {

    /**
     * Default robust estimator method for robust position estimation using ranging
     * data when no robust method is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_RANGING_ROBUST_METHOD =
            RobustEstimatorMethod.PROMedS;

    /**
     * Default robust method for coarse robust position estimation using RSSI
     * data when no robust method is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_RSSI_ROBUST_METHOD =
            RobustEstimatorMethod.PROMedS;

    /**
     * Indicates that by default located radio source position covariance is taken
     * into account (if available) to determine distance standard deviation for ranging
     * measurements.
     */
    public static final boolean DEFAULT_USE_RANGING_RADIO_SOURCE_POSITION_COVARIANCE =
            true;

    /**
     * Indicates that by default located radio source position covariance is taken
     * into account (if available) to determine distance standard deviation for RSSI
     * measurements.
     */
    public static final boolean DEFAULT_USE_RSSI_RADIO_SOURCE_POSITION_COVARIANCE =
            true;

    /**
     * Indicates that by default readings are distributed evenly among radio sources
     * taking into account quality scores of both radio sources and ranging readings.
     */
    public static final boolean DEFAULT_EVENLY_DISTRIBUTE_RANGING_READINGS = true;

    /**
     * Indicates that by default readings are distributed evenly among radio sources
     * taking into account quality scores of both radio sources and RSSI readings.
     */
    public static final boolean DEFAULT_EVENLY_DISTRIBUTE_RSSI_READINGS = true;

    /**
     * Distance standard deviation assumed for provided distances as a fallback when
     * none can be determined.
     */
    public static final double FALLBACK_DISTANCE_STANDARD_DEVIATION =
            RobustPositionEstimator.FALLBACK_DISTANCE_STANDARD_DEVIATION;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen subsamples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Indicates that result is refined by default using all found inliers.
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Indicates that by default a linear solver is used for preliminary solution
     * estimation using ranging measurements.
     * The result obtained on each preliminary solution might be later refined.
     */
    public static final boolean DEFAULT_USE_RANGING_LINEAR_SOLVER = true;

    /**
     * Indicates that by default a linear solver is used for preliminary solution
     * estimation using RSSI measurements.
     * The result obtained on each preliminary solution might be later refined.
     */
    public static final boolean DEFAULT_USE_RSSI_LINEAR_SOLVER = true;

    /**
     * Indicates that by default an homogeneous linear solver is used either to
     * estimate preliminary solutions or an initial solution for preliminary solutions
     * that will be later refined on the ranging fine estimation.
     */
    public static final boolean DEFAULT_USE_RANGING_HOMOGENEOUS_LINEAR_SOLVER = false;

    /**
     * Indicates that by default an homogeneous linear solver is used either to
     * estimate preliminary solutions or an initial solution for preliminary solutions
     * that will be later refined on the RSSI coarse estimation.
     */
    public static final boolean DEFAULT_USE_RSSI_HOMOGENEOUS_LINEAR_SOLVER = false;

    /**
     * Indicates that by default preliminary ranging solutions are refined.
     */
    public static final boolean DEFAULT_REFINE_RANGING_PRELIMINARY_SOLUTIONS = true;

    /**
     * Indicates that by default preliminary RSSI solutions are refined.
     */
    public static final boolean DEFAULT_REFINE_RSSI_PRELIMINARY_SOLUTIONS = true;

    /**
     * Internal robust estimator for position estimation using ranging readings.
     */
    protected RobustRangingPositionEstimator<P> mRangingEstimator;

    /**
     * Internal robust estimator for coarse position estimation using RSSI readings.
     */
    protected RobustRssiPositionEstimator<P> mRssiEstimator;

    /**
     * Robust method used for robust position estimation using ranging data.
     */
    protected RobustEstimatorMethod mRangingRobustMethod =
            DEFAULT_RANGING_ROBUST_METHOD;

    /**
     * Robust method used for coarse robust position estimation using RSSI data.
     */
    protected RobustEstimatorMethod mRssiRobustMethod =
            DEFAULT_RSSI_ROBUST_METHOD;

    /**
     * Size of subsets to be checked during robust estimation.
     */
    protected int mRangingPreliminarySubsetSize;

    /**
     * Size of subsets to be checked during RSSI robust estimation.
     */
    protected int mRssiPreliminarySubsetSize;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5850
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6977
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6133
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7318
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
File Line
com/irurueta/navigation/indoor/radiosource/RangingRadioSourceEstimator2D.java 186
com/irurueta/navigation/indoor/radiosource/RobustRangingRadioSourceEstimator2D.java 701
    public RadioSourceLocated<Point2D> getEstimatedRadioSource() {
        final List<? extends RangingReadingLocated<S, Point2D>> readings = getReadings();
        if (readings == null || readings.isEmpty()) {
            return null;
        }
        final S source = readings.get(0).getSource();

        final Point2D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        if (source instanceof WifiAccessPoint) {
            final WifiAccessPoint accessPoint = (WifiAccessPoint) source;
            return new WifiAccessPointLocated2D(accessPoint.getBssid(),
                    accessPoint.getFrequency(), accessPoint.getSsid(),
                    estimatedPosition, estimatedPositionCovariance);
        } else if (source instanceof Beacon) {
            final Beacon beacon = (Beacon) source;
            return new BeaconLocated2D(beacon.getIdentifiers(),
                    beacon.getTransmittedPower(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(), estimatedPosition,
                    estimatedPositionCovariance);
        } else {
            return null;
        }
    }

    /**
     * Builds an instance of a linear lateration solver if needed.
     */
    @Override
File Line
com/irurueta/navigation/indoor/radiosource/RangingRadioSourceEstimator3D.java 186
com/irurueta/navigation/indoor/radiosource/RobustRangingRadioSourceEstimator3D.java 704
    public RadioSourceLocated<Point3D> getEstimatedRadioSource() {
        final List<? extends RangingReadingLocated<S, Point3D>> readings = getReadings();
        if (readings == null || readings.isEmpty()) {
            return null;
        }
        final S source = readings.get(0).getSource();

        final Point3D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        if (source instanceof WifiAccessPoint) {
            final WifiAccessPoint accessPoint = (WifiAccessPoint) source;
            return new WifiAccessPointLocated3D(accessPoint.getBssid(),
                    accessPoint.getFrequency(), accessPoint.getSsid(),
                    estimatedPosition, estimatedPositionCovariance);
        } else if (source instanceof Beacon) {
            final Beacon beacon = (Beacon) source;
            return new BeaconLocated3D(beacon.getIdentifiers(),
                    beacon.getTransmittedPower(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(), estimatedPosition,
                    estimatedPositionCovariance);
        } else {
            return null;
        }
    }

    /**
     * Builds an instance of a linear lateration solver if needed.
     */
    @Override
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 732
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 197
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<Matrix> innerEstimator =
                new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() {
                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<Matrix> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final Matrix currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 738
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 200
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 181
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(
                        new LMedSRobustEstimatorListener<PreliminaryResult>() {
                            @Override
                            public int getTotalSamples() {
                                return mMeasurements.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return mPreliminarySubsetSize;
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<PreliminaryResult> solutions) {
                                computePreliminarySolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final PreliminaryResult currentEstimation,
                                    final int i) {
                                return computeError(mMeasurements.get(i), currentEstimation);
                            }

                            @Override
                            public boolean isReady() {
                                return LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 53
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 45
    private static final double SCALING_THRESHOLD = 2e-5;

    /**
     * Alpha threshold.
     */
    private static final double ALPHA_THRESHOLD = 1e-8;

    /**
     * Number of rows.
     */
    private static final int ROWS = 3;

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(final double timeInterval,
                         final CoordinateTransformation c,
                         final CoordinateTransformation oldC,
                         final double vx, final double vy, final double vz,
                         final double oldVx, final double oldVy, final double oldVz,
                         final double x, final double y, final double z,
                         final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(final Time timeInterval,
                         final CoordinateTransformation c,
                         final CoordinateTransformation oldC,
                         final double vx, final double vy, final double vz,
                         final double oldVx, final double oldVy, final double oldVz,
                         final double x, final double y, final double z,
                         final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(final double timeInterval,
                         final CoordinateTransformation c,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 732
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 197
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 954
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<Matrix> innerEstimator =
                new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() {
                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<Matrix> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final Matrix currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1869
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1888
                                    PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum required
     *                                  number of samples (10 or 13).
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinimumRequiredMeasurements()) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 190
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 661
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 733
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 646
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 196
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 192
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 954
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<PreliminaryResult>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mMeasurements.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 738
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1860
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 200
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 1891
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 181
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 958
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(
                        new LMedSRobustEstimatorListener<PreliminaryResult>() {
                            @Override
                            public int getTotalSamples() {
                                return mMeasurements.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return mPreliminarySubsetSize;
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<PreliminaryResult> solutions) {
                                computePreliminarySolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final PreliminaryResult currentEstimation,
                                    final int i) {
                                return computeError(mMeasurements.get(i), currentEstimation);
                            }

                            @Override
                            public boolean isReady() {
                                return LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 4033
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 4096
                                    PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < TurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/indoor/position/LinearMixedPositionEstimator2D.java 155
com/irurueta/navigation/indoor/position/LinearRangingAndRssiPositionEstimator2D.java 152
com/irurueta/navigation/indoor/position/LinearRangingPositionEstimator2D.java 152
com/irurueta/navigation/indoor/position/LinearRssiPositionEstimator2D.java 154
            final MixedPositionEstimatorListener<Point2D> listener) {
        super(listener);
        init();
        internalSetSources(sources);
        internalSetFingerprint(fingerprint);
    }

    /**
     * Gets estimated position.
     *
     * @return estimated position.
     */
    @Override
    public Point2D getEstimatedPosition() {
        if (mEstimatedPositionCoordinates == null) {
            return null;
        }

        final InhomogeneousPoint2D result = new InhomogeneousPoint2D();
        getEstimatedPosition(result);
        return result;
    }

    /**
     * Sets positions and distances on internal lateration solver.
     *
     * @param positions positions to be set.
     * @param distances distances to be set.
     * @throws IllegalArgumentException if something fails.
     */
    @Override
    @SuppressWarnings("Duplicates")
    protected void setPositionsAndDistances(
            final List<Point2D> positions, final List<Double> distances) {
        final int size = positions.size();
        Point2D[] positionsArray = new InhomogeneousPoint2D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
        }

        try {
            mHomogeneousTrilaterationSolver.setPositionsAndDistances(positionsArray,
                    distancesArray);
            mInhomogeneousTrilaterationSolver.setPositionsAndDistances(positionsArray,
                    distancesArray);
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }

    /**
     * Initializes lateration solver.
     */
    private void init() {
        mHomogeneousTrilaterationSolver = new HomogeneousLinearLeastSquaresLateration2DSolver(
                mLaterationSolverListener);
        mInhomogeneousTrilaterationSolver = new InhomogeneousLinearLeastSquaresLateration2DSolver(
                mLaterationSolverListener);
    }
}
File Line
com/irurueta/navigation/indoor/position/LinearMixedPositionEstimator3D.java 155
com/irurueta/navigation/indoor/position/LinearRangingAndRssiPositionEstimator3D.java 155
com/irurueta/navigation/indoor/position/LinearRssiPositionEstimator3D.java 154
            final MixedPositionEstimatorListener<Point3D> listener) {
        super(listener);
        init();
        internalSetSources(sources);
        internalSetFingerprint(fingerprint);
    }

    /**
     * Gets estimated position.
     * @return estimated position.
     */
    @Override
    public Point3D getEstimatedPosition() {
        if(mEstimatedPositionCoordinates == null) {
            return null;
        }

        final InhomogeneousPoint3D result = new InhomogeneousPoint3D();
        getEstimatedPosition(result);
        return result;
    }

    /**
     * Sets positions and distances on internal lateration solver.
     * @param positions positions to be set.
     * @param distances distances to be set.
     * @throws IllegalArgumentException if something fails.
     */
    @Override
    @SuppressWarnings("Duplicates")
    protected void setPositionsAndDistances(
            final List<Point3D> positions, final List<Double> distances) {
        final int size = positions.size();
        Point3D[] positionsArray = new InhomogeneousPoint3D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
        }

        try {
            mHomogeneousTrilaterationSolver.setPositionsAndDistances(positionsArray,
                    distancesArray);
            mInhomogeneousTrilaterationSolver.setPositionsAndDistances(positionsArray,
                    distancesArray);
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }

    /**
     * Initializes lateration solver.
     */
    private void init() {
        mHomogeneousTrilaterationSolver = new HomogeneousLinearLeastSquaresLateration3DSolver(
                mLaterationSolverListener);
        mInhomogeneousTrilaterationSolver = new InhomogeneousLinearLeastSquaresLateration3DSolver(
                mLaterationSolverListener);
    }
}
File Line
com/irurueta/navigation/indoor/position/RobustRangingAndRssiPositionEstimator3D.java 985
com/irurueta/navigation/indoor/position/RobustRangingPositionEstimator3D.java 994
com/irurueta/navigation/indoor/position/RobustRssiPositionEstimator3D.java 985
    protected void setPositionsDistancesDistanceStandardDeviationsAndQualityScores(
            final List<Point3D> positions, List<Double> distances,
            final List<Double> distanceStandardDeviations,
            final List<Double> distanceQualityScores) {
        final int size = positions.size();
        Point3D[] positionsArray = new InhomogeneousPoint3D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];

        double[] qualityScoresArray = null;
        if (distanceQualityScores != null) {
            qualityScoresArray = new double[size];
        }

        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);

            if (qualityScoresArray != null) {
                qualityScoresArray[i] = distanceQualityScores.get(i);
            }
        }

        try {
            mLaterationSolver.setPositionsDistancesAndStandardDeviations(
                    positionsArray, distancesArray, distanceStandardDeviationsArray);

            if (qualityScoresArray != null) {
                mLaterationSolver.setQualityScores(qualityScoresArray);
            }
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 1058
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 549
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 1078
                                            PROSACRobustRangingAndRssiRadioSourceEstimator2D.this,
                                            progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point2D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than required minimum.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinReadings()) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 548
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 1075
                                            PROSACRobustRangingRadioSourceEstimator3D.this,
                                            progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point3D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }


    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinReadings()) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 853
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 995
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double omegaMeasX = measuredKinematics.getAngularRateX();
            final double omegaMeasY = measuredKinematics.getAngularRateY();
            final double omegaMeasZ = measuredKinematics.getAngularRateZ();

            final double omegaTrueX = expectedKinematics.getAngularRateX();
            final double omegaTrueY = expectedKinematics.getAngularRateY();
            final double omegaTrueZ = expectedKinematics.getAngularRateZ();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, 1.0);
            a.setElementAt(i, 3, omegaTrueX);
            a.setElementAt(i, 6, omegaTrueY);
            a.setElementAt(i, 7, omegaTrueZ);
            a.setElementAt(i, 9, fTrueX);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4961
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6283
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5201
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6583
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator.java 1145
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator.java 2453
    private RangingReadingLocated<S, P> createRangingReading(
            final RangingAndRssiReadingLocated<S, P> reading) {
        return new RangingReadingLocated<>(reading.getSource(),
                reading.getDistance(), reading.getPosition(),
                reading.getDistanceStandardDeviation(),
                reading.getPositionCovariance());
    }

    /**
     * Creates an RSSI reading from a ranging and RSSI reading.
     *
     * @param reading input reading to convert from.
     * @return an RSSI reading containing only the RSSI data of input reading.
     */
    private RssiReadingLocated<S, P> createRssiReading(
            final RangingAndRssiReadingLocated<S, P> reading) {
        return new RssiReadingLocated<>(reading.getSource(),
                reading.getRssi(), reading.getPosition(),
                reading.getRssiStandardDeviation(),
                reading.getPositionCovariance());
    }

    /**
     * Checks number of available ranging readings and number of available RSSI readings. Also determines
     * whether position must be estimated using ranging data or RSSI data.
     *
     * @param readings readings to be checked.
     */
    private void checkReadings(final List<? extends ReadingLocated<P>> readings) {
        mNumRangingReadings = mNumRssiReadings = 0;

        if (readings == null) {
            return;
        }

        for (final ReadingLocated<P> reading : readings) {
            if (reading instanceof RangingReadingLocated) {
                mNumRangingReadings++;

            } else if (reading instanceof RssiReadingLocated) {
                mNumRssiReadings++;

            } else if (reading instanceof RangingAndRssiReadingLocated) {
                mNumRangingReadings++;
                mNumRssiReadings++;
            }
        }

        mRssiPositionEnabled = mNumRangingReadings < getMinRangingReadings();
    }
}
File Line
com/irurueta/navigation/inertial/calibration/StandardDeviationFrameBodyKinematics.java 1310
com/irurueta/navigation/inertial/calibration/StandardDeviationTimedBodyKinematics.java 359
            final StandardDeviationFrameBodyKinematics input) {
        copyFrom(input);
    }

    /**
     * Gets standard deviation of measured specific force expressed in meters per squared
     * second (m/s^2).
     *
     * @return standard deviation of measured specific force.
     */
    public double getSpecificForceStandardDeviation() {
        return mSpecificForceStandardDeviation;
    }

    /**
     * Sets standard deviation of measured specific force expressed in meters per squared
     * second (m/s^2).
     *
     * @param specificForceStandardDeviation standard deviation of measured specific force.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setSpecificForceStandardDeviation(
            final double specificForceStandardDeviation) {
        if (specificForceStandardDeviation < 0.0) {
            throw new IllegalArgumentException();
        }

        mSpecificForceStandardDeviation = specificForceStandardDeviation;
    }

    /**
     * Gets standard deviation of measured specific force.
     *
     * @return standard deviation of measured specific force.
     */
    public Acceleration getSpecificForceStandardDeviationAsAcceleration() {
        return new Acceleration(mSpecificForceStandardDeviation,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets standard deviation of measured specific force.
     *
     * @param result instance where standard deviation of measured specific force will be
     *               stored.
     */
    public void getSpecificForceStandardDeviationAsAcceleration(
            final Acceleration result) {
        result.setValue(mSpecificForceStandardDeviation);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets standard deviation of measured specific force.
     *
     * @param specificForceStandardDeviation standard deviation of measured specific force.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setSpecificForceStandardDeviation(
            final Acceleration specificForceStandardDeviation) {
        setSpecificForceStandardDeviation(convertAcceleration(
                specificForceStandardDeviation));
    }

    /**
     * Gets standard deviation of measured angular rate expressed in radians per second (rad/s).
     *
     * @return standard deviation of measured angular rate.
     */
    public double getAngularRateStandardDeviation() {
        return mAngularRateStandardDeviation;
    }

    /**
     * Sets standard deviation of measured angular rate expressed in radians per second (rad/s).
     *
     * @param angularRateStandardDeviation standard deviation of measured angular rate.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setAngularRateStandardDeviation(final double angularRateStandardDeviation) {
        if (angularRateStandardDeviation < 0.0) {
            throw new IllegalArgumentException();
        }

        mAngularRateStandardDeviation = angularRateStandardDeviation;
    }

    /**
     * Gets standard deviation of measured angular rate.
     *
     * @return standard deviation of measured angular rate.
     */
    public AngularSpeed getAngularRateStandardDeviationAsAngularSpeed() {
        return new AngularSpeed(mAngularRateStandardDeviation,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets standard deviation of measured angular rate.
     *
     * @param result instance where standard deviation of measured angular rate will be
     *               stored.
     */
    public void getAngularRateStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        result.setValue(mAngularRateStandardDeviation);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets standard deviation of measured angular rate.
     *
     * @param angularRateStandardDeviation standard deviation of measured angular rate.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setAngularRateStandardDeviation(
            final AngularSpeed angularRateStandardDeviation) {
        setAngularRateStandardDeviation(convertAngularSpeed(
                angularRateStandardDeviation));
    }

    /**
     * Copies data of provided instance into this instance.
     *
     * @param input instance to copy data from.
     */
    public void copyFrom(final StandardDeviationFrameBodyKinematics input) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 536
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1771
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 544
                                    PROMedSRobustKnownFrameAccelerometerCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 454
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 214
                         final ECEFFrame frame, final ECEFFrame oldFrame,
                         final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldFrame, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(final double timeInterval,
                         final CoordinateTransformation c,
                         final CoordinateTransformation oldC,
                         final Speed vx, final Speed vy, final Speed vz,
                         final Speed oldVx, final Speed oldVy, final Speed oldVz,
                         final double x, final double y, final double z,
                         final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(final Time timeInterval,
                         final CoordinateTransformation c,
                         final CoordinateTransformation oldC,
                         final Speed vx, final Speed vy, final Speed vz,
                         final Speed oldVx, final Speed oldVy, final Speed oldVz,
                         final double x, final double y, final double z,
                         final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz,
                oldVx, oldVy, oldVz, x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param position     cartesian body position resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(final double timeInterval,
                         final CoordinateTransformation c,
File Line
com/irurueta/navigation/indoor/fingerprint/SecondOrderNonLinearFingerprintPositionEstimator3D.java 157
com/irurueta/navigation/indoor/fingerprint/ThirdOrderNonLinearFingerprintPositionEstimator3D.java 154
    }

    /**
     * Evaluates a non-linear multi dimension function at provided point using
     * provided parameters and returns its evaluation and derivatives of the
     * function respect the function parameters.
     *
     * @param i           number of sample being evaluated.
     * @param point       point where function will be evaluated.
     * @param params      initial parameters estimation to be tried. These will
     *                    change as the Levenberg-Marquard algorithm iterates to the best solution.
     *                    These are used as input parameters along with point to evaluate function.
     * @param derivatives partial derivatives of the function respect to each
     *                    provided parameter.
     * @return function evaluation at provided point.
     */
    @Override
    @SuppressWarnings("Duplicates")
    protected double evaluate(
            final int i, final double[] point, final double[] params,
            final double[] derivatives) {
        //This method implements received power at point pi = (xi, yi, zi) and its derivatives

        //Pr(pi) = Pr(p1)
        // - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
        // - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
        // - 10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1)
        // - 5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)^2
        // - 5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*d1a^4)*(yi - y1)^2
        // - 5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*d1a^4)*(zi - z1)^2
        // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)*(xi - x1)*(yi - y1)
        // + 20*n*(y1 - ya)*(z1 - za)/(ln(10)*d1a^4)*(yi - y1)*(zi - z1)
        // + 20*n*(x1 - xa)*(z1 - za)/(ln(10)*d1a^4)*(xi - x1)*(zi - z1)

        final double xi = params[0];
        final double yi = params[1];
        final double zi = params[2];

        //received power
        final double pr = point[0];

        //fingerprint coordinates
        final double x1 = point[1];
        final double y1 = point[2];
        final double z1 = point[3];

        //radio source coordinates
        final double xa = point[4];
        final double ya = point[5];
        final double za = point[6];

        //path loss exponent
        final double n = point[7];

        final double ln10 = Math.log(10.0);

        final double diffXi1 = xi - x1;
        final double diffYi1 = yi - y1;
        final double diffZi1 = zi - z1;

        final double diffX1a = x1 - xa;
        final double diffY1a = y1 - ya;
        final double diffZ1a = z1 - za;

        final double diffXi12 = diffXi1 * diffXi1;
        final double diffYi12 = diffYi1 * diffYi1;
        final double diffZi12 = diffZi1 * diffZi1;

        final double diffX1a2 = diffX1a * diffX1a;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5460
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6782
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5561
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5757
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6883
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7074
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5723
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7105
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5830
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6029
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7212
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7421
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5121
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6443
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5289
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6611
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7578
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8986
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5364
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6746
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5540
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6922
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7959
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9446
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1889
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 561
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1912
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1791
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 4055
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 568
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 4119
                                    PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.this, progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum required
     *                                  number of samples (10 or 13).
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinimumRequiredMeasurements()) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8405
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8606
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8828
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9044
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/indoor/position/SequentialRobustMixedPositionEstimator.java 2043
com/irurueta/navigation/indoor/position/SequentialRobustRangingAndRssiPositionEstimator.java 1929
            final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint) {
        if (fingerprint == null) {
            throw new IllegalArgumentException();
        }

        mFingerprint = fingerprint;
    }

    /**
     * Sets quality scores corresponding to each provided located radio source.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param sourceQualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length is
     *                                  smaller than 3 samples for 2D or 4 samples for 3D.
     */
    private void internalSetSourceQualityScores(final double[] sourceQualityScores) {
        if (sourceQualityScores == null ||
                sourceQualityScores.length < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mSourceQualityScores = sourceQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided reading within provided
     * fingerprint.
     * This method is used internally and does not check whether instance is locked
     * or not.
     *
     * @param fingerprintReadingsQualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than 3 samples for 2D or 4 samples for 3D.
     */
    private void internalSetFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) {
        if (fingerprintReadingsQualityScores == null ||
                fingerprintReadingsQualityScores.length < getMinRequiredSources()) {
            throw new IllegalArgumentException();
        }

        mFingerprintReadingsQualityScores = fingerprintReadingsQualityScores;
    }

    /**
     * Creates a ranging reading from a ranging and RSSI reading.
     *
     * @param reading input reading to convert from.
     * @return a ranging reading containing only the ranging data of input reading.
     */
    private RangingReading<RadioSource> createRangingReading(
            final RangingAndRssiReading<? extends RadioSource> reading) {
        return new RangingReading<>(reading.getSource(),
                reading.getDistance(),
                reading.getDistanceStandardDeviation(),
                reading.getNumAttemptedMeasurements(),
                reading.getNumSuccessfulMeasurements());
    }

    /**
     * Creates an RSSI reading from a ranging and RSSI reading.
     *
     * @param reading input reading to convert from.
     * @return an RSSI reading containing only the RSSI data of input reading.
     */
    private RssiReading<RadioSource> createRssiReading(
            final RangingAndRssiReading<? extends RadioSource> reading) {
        return new RssiReading<>(reading.getSource(), reading.getRssi(),
                reading.getRssiStandardDeviation());
    }
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator2D.java 2947
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator2D.java 2927
                    mUseReadingPositionCovariances);

            mInnerEstimator.estimate();

            final Point2D estimatedPosition = mInnerEstimator.getEstimatedPosition();
            final double estimatedTransmittedPowerdBm =
                    mInnerEstimator.getEstimatedTransmittedPowerdBm();
            final double estimatedPathLossExponent =
                    mInnerEstimator.getEstimatedPathLossExponent();
            solutions.add(new Solution<>(estimatedPosition,
                    estimatedTransmittedPowerdBm, estimatedPathLossExponent));
        } catch (final NavigationException ignore) {
            //if anything fails, no solution is added
        }
    }

    /**
     * Attempts to refine estimated position and transmitted power contained in
     * provided solution if refinement is requested.
     * This method sets a refined result and transmitted power or provided input
     * result if refinement is not requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined result.
     * solution if not requested or refinement failed.
     *
     * @param result result to be refined.
     */
    protected void attemptRefine(final Solution<Point2D> result) {
        final Point2D initialPosition = result.getEstimatedPosition();
        final double initialTransmittedPowerdBm =
                result.getEstimatedTransmittedPowerdBm();
        final double initialPathLossExponent = result.getEstimatedPathLossExponent();

        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mReadings.size();

            mInnerReadings.clear();

            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    //sample is inlier
                    mInnerReadings.add(mReadings.get(i));
                }
            }

            try {
                mInnerEstimator.setInitialPosition(initialPosition);
                mInnerEstimator.setInitialTransmittedPowerdBm(initialTransmittedPowerdBm);
                mInnerEstimator.setInitialPathLossExponent(initialPathLossExponent);
                mInnerEstimator.setTransmittedPowerEstimationEnabled(
                        mTransmittedPowerEstimationEnabled);
                mInnerEstimator.setPathLossEstimationEnabled(
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator3D.java 2954
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator3D.java 2928
                    mUseReadingPositionCovariances);

            mInnerEstimator.estimate();

            final Point3D estimatedPosition = mInnerEstimator.getEstimatedPosition();
            final double estimatedTransmittedPowerdBm =
                    mInnerEstimator.getEstimatedTransmittedPowerdBm();
            final double estimatedPathLossExponent =
                    mInnerEstimator.getEstimatedPathLossExponent();
            solutions.add(new Solution<>(estimatedPosition,
                    estimatedTransmittedPowerdBm, estimatedPathLossExponent));
        } catch (final NavigationException ignore) {
            //if anything fails, no solution is added
        }
    }

    /**
     * Attempts to refine estimated position and transmitted power contained in
     * provided solution if refinement is requested.
     * This method sets a refined result and transmitted power or provided input
     * result if refinement is not requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined result.
     * solution if not requested or refinement failed.
     *
     * @param result result to be refined.
     */
    protected void attemptRefine(final Solution<Point3D> result) {
        final Point3D initialPosition = result.getEstimatedPosition();
        final double initialTransmittedPowerdBm =
                result.getEstimatedTransmittedPowerdBm();
        final double initialPathLossExponent = result.getEstimatedPathLossExponent();

        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mReadings.size();

            mInnerReadings.clear();

            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    //sample is inlier
                    mInnerReadings.add(mReadings.get(i));
                }
            }

            try {
                mInnerEstimator.setInitialPosition(initialPosition);
                mInnerEstimator.setInitialTransmittedPowerdBm(initialTransmittedPowerdBm);
                mInnerEstimator.setInitialPathLossExponent(initialPathLossExponent);
                mInnerEstimator.setTransmittedPowerEstimationEnabled(
                        mTransmittedPowerEstimationEnabled);
                mInnerEstimator.setPathLossEstimationEnabled(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1350
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 872
        final double myz = unknowns.getElementAtIndex(5);

        fillMa(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException if there are numerical errors.
     */
    private void calibrateGeneral() throws AlgebraException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // Hence:
        //  [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        //  [fmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [ftruey]
        //  [fmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [ftruez]

        //  [fmeasx] = [bx] +   [1+sx   mxy     mxz ][ftruex]
        //  [fmeasy]   [by]     [myx    1+sy    myz ][ftruey]
        //  [fmeasz]   [bz]     [mzx    mzy     1+sz][ftruez]

        //  fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + myx * ftruex + (1+sy) * ftruey + myz * ftruez
        //  fmeasz = bz + mzx * ftruex + mzy * ftruey + (1+sz) * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
        // Reordering:
        //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
        //  fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

        //  fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy - ftruey - by = myx * ftruex + sy * ftruey + myz * ftruez
        //  fmeasz - ftruez - bz = mzx * ftruex + mzy * ftruey + sz * ftruez

        // [ftruex  0       0       ftruey  ftruez  0       0       0       0     ][sx ] = [fmeasx - ftruex - bx]
        // [0       ftruey  0       0       0       ftruex  ftruez  0       0     ][sy ]   [fmeasy - ftruey - by]
        // [0       0       ftruez  0       0       0       0       ftruex  ftruey][sz ]   [fmeasz - ftruez - bz]
        //                                                                         [mxy]
        //                                                                         [mxz]
        //                                                                         [myx]
        //                                                                         [myz]
        //                                                                         [mzx]
        //                                                                         [mzy]

        final BodyKinematics expectedKinematics = new BodyKinematics();

        final int rows = EQUATIONS_PER_MEASUREMENT * mMeasurements.size();
        final Matrix a = new Matrix(rows, GENERAL_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double fMeasX = measuredKinematics.getFx();
            final double fMeasY = measuredKinematics.getFy();
            final double fMeasZ = measuredKinematics.getFz();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, fTrueX);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 728
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 193
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 954
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<Matrix> innerEstimator =
                new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Matrix>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<Matrix> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final Matrix currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7671
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7842
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8106
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9514
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9076
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9250
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8054
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8231
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8508
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9998
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9541
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9718
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator2D.java 381
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator2D.java 879
    public RadioSourceLocated<Point2D> getEstimatedRadioSource() {
        final List<? extends ReadingLocated<Point2D>> readings = getReadings();
        if (readings == null || readings.isEmpty()) {
            return null;
        }

        final S source;
        final ReadingLocated<Point2D> reading = readings.get(0);
        if (reading instanceof RangingReadingLocated) {
            source = ((RangingReadingLocated<S, Point2D>) reading).getSource();
        } else if (reading instanceof RssiReadingLocated) {
            source = ((RssiReadingLocated<S, Point2D>) reading).getSource();
        } else if (reading instanceof RangingAndRssiReadingLocated) {
            source = ((RangingAndRssiReadingLocated<S, Point2D>) reading).getSource();
        } else {
            return null;
        }

        final Point2D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        final Double transmittedPowerdBm = getEstimatedTransmittedPowerdBm();

        final Double transmittedPowerVariance = getEstimatedTransmittedPowerVariance();
        final Double transmittedPowerStandardDeviation = transmittedPowerVariance != null ?
                Math.sqrt(transmittedPowerVariance) : null;

        final double pathLossExponent = getEstimatedPathLossExponent();
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator3D.java 385
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator3D.java 879
    public RadioSourceLocated<Point3D> getEstimatedRadioSource() {
        final List<? extends ReadingLocated<Point3D>> readings = getReadings();
        if (readings == null || readings.isEmpty()) {
            return null;
        }

        final S source;
        final ReadingLocated<Point3D> reading = readings.get(0);
        if (reading instanceof RangingReadingLocated) {
            source = ((RangingReadingLocated<S, Point3D>) reading).getSource();
        } else if (reading instanceof RssiReadingLocated) {
            source = ((RssiReadingLocated<S, Point3D>) reading).getSource();
        } else if (reading instanceof RangingAndRssiReadingLocated) {
            source = ((RangingAndRssiReadingLocated<S, Point3D>) reading).getSource();
        } else {
            return null;
        }

        final Point3D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        final Double transmittedPowerdBm = getEstimatedTransmittedPowerdBm();

        final Double transmittedPowerVariance = getEstimatedTransmittedPowerVariance();
        final Double transmittedPowerStandardDeviation = transmittedPowerVariance != null ?
                Math.sqrt(transmittedPowerVariance) : null;

        final double pathLossExponent = getEstimatedPathLossExponent();
File Line
com/irurueta/navigation/indoor/position/NonLinearMixedPositionEstimator2D.java 300
com/irurueta/navigation/indoor/position/NonLinearRangingAndRssiPositionEstimator2D.java 300
com/irurueta/navigation/indoor/position/NonLinearRangingPositionEstimator2D.java 303
com/irurueta/navigation/indoor/position/NonLinearRssiPositionEstimator2D.java 304
    public Point2D getEstimatedPosition() {
        if (mEstimatedPositionCoordinates == null) {
            return null;
        }

        final InhomogeneousPoint2D result = new InhomogeneousPoint2D();
        getEstimatedPosition(result);
        return result;
    }

    /**
     * Sets positions, distances and standard deviations of distances on internal
     * lateration solver.
     *
     * @param positions                  positions to be set.
     * @param distances                  distances to be set.
     * @param distanceStandardDeviations standard deviations of distances to be set.
     */
    @Override
    @SuppressWarnings("Duplicates")
    protected void setPositionsDistancesAndDistanceStandardDeviations(
            final List<Point2D> positions, final List<Double> distances,
            final List<Double> distanceStandardDeviations) {

        final int size = positions.size();
        Point2D[] positionsArray = new InhomogeneousPoint2D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];
        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);
        }

        try {
            mTrilaterationSolver.setPositionsDistancesAndStandardDeviations(
                    positionsArray, distancesArray, distanceStandardDeviationsArray);
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }

    /**
     * Initializes lateration solver.
     */
    private void init() {
        mTrilaterationSolver = new NonLinearLeastSquaresLateration2DSolver(
                mLaterationSolverListener);
    }
}
File Line
com/irurueta/navigation/indoor/position/NonLinearRangingAndRssiPositionEstimator3D.java 303
com/irurueta/navigation/indoor/position/NonLinearRangingPositionEstimator3D.java 303
com/irurueta/navigation/indoor/position/NonLinearRssiPositionEstimator3D.java 304
    public Point3D getEstimatedPosition() {
        if (mEstimatedPositionCoordinates == null) {
            return null;
        }

        final InhomogeneousPoint3D result = new InhomogeneousPoint3D();
        getEstimatedPosition(result);
        return result;
    }

    /**
     * Sets positions, distances and standard deviations of distances on internal
     * lateration solver.
     *
     * @param positions                  positions to be set.
     * @param distances                  distances to be set.
     * @param distanceStandardDeviations standard deviations of distances to be set.
     */
    @Override
    @SuppressWarnings("Duplicates")
    protected void setPositionsDistancesAndDistanceStandardDeviations(
            final List<Point3D> positions, final List<Double> distances,
            final List<Double> distanceStandardDeviations) {

        final int size = positions.size();
        Point3D[] positionsArray = new InhomogeneousPoint3D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];
        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);
        }

        try {
            mTrilaterationSolver.setPositionsDistancesAndStandardDeviations(
                    positionsArray, distancesArray, distanceStandardDeviationsArray);
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }

    /**
     * Initializes lateration solver.
     */
    private void init() {
        mTrilaterationSolver = new NonLinearLeastSquaresLateration3DSolver(
                mLaterationSolverListener);
    }
}
File Line
com/irurueta/navigation/inertial/calibration/StandardDeviationBodyKinematics.java 169
com/irurueta/navigation/inertial/calibration/StandardDeviationFrameBodyKinematics.java 1312
com/irurueta/navigation/inertial/calibration/StandardDeviationTimedBodyKinematics.java 361
    }

    /**
     * Gets standard deviation of measured specific force expressed in meters per squared
     * second (m/s^2).
     *
     * @return standard deviation of measured specific force.
     */
    public double getSpecificForceStandardDeviation() {
        return mSpecificForceStandardDeviation;
    }

    /**
     * Sets standard deviation of measured specific force expressed in meters per squared
     * second (m/s^2).
     *
     * @param specificForceStandardDeviation standard deviation of measured specific force.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setSpecificForceStandardDeviation(
            final double specificForceStandardDeviation) {
        if (specificForceStandardDeviation < 0.0) {
            throw new IllegalArgumentException();
        }

        mSpecificForceStandardDeviation = specificForceStandardDeviation;
    }

    /**
     * Gets standard deviation of measured specific force.
     *
     * @return standard deviation of measured specific force.
     */
    public Acceleration getSpecificForceStandardDeviationAsAcceleration() {
        return new Acceleration(mSpecificForceStandardDeviation,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets standard deviation of measured specific force.
     *
     * @param result instance where standard deviation of measured specific force will be
     *               stored.
     */
    public void getSpecificForceStandardDeviationAsAcceleration(
            final Acceleration result) {
        result.setValue(mSpecificForceStandardDeviation);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets standard deviation of measured specific force.
     *
     * @param specificForceStandardDeviation standard deviation of measured specific force.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setSpecificForceStandardDeviation(
            final Acceleration specificForceStandardDeviation) {
        setSpecificForceStandardDeviation(convertAcceleration(
                specificForceStandardDeviation));
    }

    /**
     * Gets standard deviation of measured angular rate expressed in radians per second (rad/s).
     *
     * @return standard deviation of measured angular rate.
     */
    public double getAngularRateStandardDeviation() {
        return mAngularRateStandardDeviation;
    }

    /**
     * Sets standard deviation of measured angular rate expressed in radians per second (rad/s).
     *
     * @param angularRateStandardDeviation standard deviation of measured angular rate.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setAngularRateStandardDeviation(final double angularRateStandardDeviation) {
        if (angularRateStandardDeviation < 0.0) {
            throw new IllegalArgumentException();
        }

        mAngularRateStandardDeviation = angularRateStandardDeviation;
    }

    /**
     * Gets standard deviation of measured angular rate.
     *
     * @return standard deviation of measured angular rate.
     */
    public AngularSpeed getAngularRateStandardDeviationAsAngularSpeed() {
        return new AngularSpeed(mAngularRateStandardDeviation,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets standard deviation of measured angular rate.
     *
     * @param result instance where standard deviation of measured angular rate will be
     *               stored.
     */
    public void getAngularRateStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        result.setValue(mAngularRateStandardDeviation);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets standard deviation of measured angular rate.
     *
     * @param angularRateStandardDeviation standard deviation of measured angular rate.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setAngularRateStandardDeviation(
            final AngularSpeed angularRateStandardDeviation) {
        setAngularRateStandardDeviation(convertAngularSpeed(
                angularRateStandardDeviation));
    }

    /**
     * Copies data of provided instance into this instance.
     *
     * @param input instance to copy data from.
     */
    public void copyFrom(final StandardDeviationBodyKinematics input) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7356
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7501
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7752
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9157
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7925
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9333
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7931
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8112
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8764
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8911
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9339
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9520
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7719
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7878
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8136
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9623
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8316
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9804
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by
     *                                      this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8322
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8514
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by
     *                                      this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9209
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9365
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9810
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10004
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 2432
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 2315
            final PreliminaryResult preliminaryResult) {
        // The magnetometer model is:
        // mBmeas = ba + (I + Mm) * mBtrue

        // Hence:
        //  [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        //  [mBmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [mBtruey]
        //  [mBmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [mBtruez]

        final BodyMagneticFluxDensity measuredMagneticFluxDensity =
                measurement.getMagneticFluxDensity();
        final ECEFFrame ecefFrame = measurement.getFrame();

        final NEDFrame nedFrame = ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(ecefFrame);
        final double year = measurement.getYear();

        final double latitude = nedFrame.getLatitude();
        final double longitude = nedFrame.getLongitude();
        final double height = nedFrame.getHeight();

        final NEDMagneticFluxDensity earthB = mWmmEstimator.estimate(
                latitude, longitude, height, year);

        final CoordinateTransformation cbn = new CoordinateTransformation(
                FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final CoordinateTransformation cnb = new CoordinateTransformation(
                FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);

        nedFrame.getCoordinateTransformation(cbn);
        cbn.inverse(cnb);

        final BodyMagneticFluxDensity expectedMagneticFluxDensity =
                BodyMagneticFluxDensityEstimator.estimate(earthB, cnb);

        final double bMeasX1 = measuredMagneticFluxDensity.getBx();
        final double bMeasY1 = measuredMagneticFluxDensity.getBy();
        final double bMeasZ1 = measuredMagneticFluxDensity.getBz();

        final double bTrueX = expectedMagneticFluxDensity.getBx();
        final double bTrueY = expectedMagneticFluxDensity.getBy();
        final double bTrueZ = expectedMagneticFluxDensity.getBz();
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator2D.java 838
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator3D.java 836
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator2D.java 850
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator3D.java 850
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator2D.java 1033
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator2D.java 527
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator2D.java 1044
                                            PROMedSRobustRangingAndRssiRadioSourceEstimator2D.this,
                                            progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;

            //inlier thresholds are disabled to obtain a less restrictive amount of inliers
            innerEstimator.setUseInlierThresholds(false);

            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point2D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinReadings()) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator3D.java 1031
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator3D.java 526
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator3D.java 1045
                                            PROMedSRobustRangingAndRssiRadioSourceEstimator3D.this,
                                            progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;

            //inlier thresholds are disabled to obtain a less restrictive amount of inliers
            innerEstimator.setUseInlierThresholds(false);

            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point3D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinReadings()) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6926
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3530
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5045
                        return evaluateGeneral(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double m11 = result[0];
        final double m21 = result[1];
        final double m31 = result[2];

        final double m12 = result[3];
        final double m22 = result[4];
        final double m32 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, m21);
        m.setElementAtIndex(2, m31);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, m32);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        setResult(m);
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws FittingException                         if Levenberg-Marquardt fails for numerical reasons.
     * @throws AlgebraException                         if there are numerical instabilities that prevent
     *                                                  matrix inversion.
     * @throws com.irurueta.numerical.NotReadyException never happens.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1319
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1451
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double omegaMeasX = measuredKinematics.getAngularRateX();
            final double omegaMeasY = measuredKinematics.getAngularRateY();
            final double omegaMeasZ = measuredKinematics.getAngularRateZ();

            final double omegaTrueX = expectedKinematics.getAngularRateX();
            final double omegaTrueY = expectedKinematics.getAngularRateY();
            final double omegaTrueZ = expectedKinematics.getAngularRateZ();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, omegaTrueX);
            a.setElementAt(i, 3, omegaTrueY);
            a.setElementAt(i, 4, omegaTrueZ);
            a.setElementAt(i, 6, fTrueX);
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 1415
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 749
                                               final ECEFFrame frame,
                                               final CoordinateTransformation oldC,
                                               final Speed oldVx, final Speed oldVy, final Speed oldVz) {
        return estimateKinematicsAndReturnNew(timeInterval, frame, oldC, oldVx, oldVy, oldVz);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(final double timeInterval,
                                               final CoordinateTransformation c,
                                               final CoordinateTransformation oldC,
                                               final double vx, final double vy, final double vz,
                                               final double oldVx, final double oldVy, final double oldVz,
                                               final Point3D position) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz,
                oldVx, oldVy, oldVz, position);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(final Time timeInterval,
                                               final CoordinateTransformation c,
                                               final CoordinateTransformation oldC,
                                               final double vx, final double vy, final double vz,
                                               final double oldVx, final double oldVy, final double oldVz,
                                               final Point3D position) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                position);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(final double timeInterval,
                                               final CoordinateTransformation c,
                                               final CoordinateTransformation oldC,
                                               final ECEFVelocity velocity,
File Line
com/irurueta/navigation/indoor/position/RobustMixedPositionEstimator3D.java 994
com/irurueta/navigation/indoor/position/RobustRangingAndRssiPositionEstimator3D.java 986
com/irurueta/navigation/indoor/position/RobustRangingPositionEstimator3D.java 995
com/irurueta/navigation/indoor/position/RobustRssiPositionEstimator3D.java 986
            final List<Point3D> positions, final List<Double> distances,
            final List<Double> distanceStandardDeviations,
            final List<Double> distanceQualityScores) {
        final int size = positions.size();
        Point3D[] positionsArray = new InhomogeneousPoint3D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];

        double[] qualityScoresArray = null;
        if (distanceQualityScores != null) {
            qualityScoresArray = new double[size];
        }

        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);

            if (qualityScoresArray != null) {
                qualityScoresArray[i] = distanceQualityScores.get(i);
            }
        }

        try {
            mLaterationSolver.setPositionsDistancesAndStandardDeviations(
                    positionsArray, distancesArray, distanceStandardDeviationsArray);

            if (qualityScoresArray != null) {
                mLaterationSolver.setQualityScores(qualityScoresArray);
            }
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3948
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5571
                angularSpeed.getUnit(), AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Makes proper conversion of internal cross-coupling, bias and g-dependent
     * cross bias matrices.
     *
     * @param m internal scaling and cross-coupling matrix.
     * @param b internal bias matrix.
     * @param g internal g-dependent cross bias matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m, final Matrix b, final Matrix g)
            throws AlgebraException {
        setResult(m, b);

        // Gg = M*G
        m.multiply(g, mEstimatedGg);
    }

    /**
     * Makes proper conversion of internal cross-coupling and bias matrices.
     *
     * @param m internal scaling and cross-coupling matrix.
     * @param b internal bias matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m, final Matrix b) throws AlgebraException {
        // Because:
        // M = I + Mg
        // b = M^-1*bg

        // Then:
        // Mg = M - I
        // bg = M*b

        if (mEstimatedBiases == null) {
            mEstimatedBiases = new double[BodyKinematics.COMPONENTS];
        }

        final Matrix bg = m.multiplyAndReturnNew(b);
        bg.toArray(mEstimatedBiases);

        if (mEstimatedMg == null) {
            mEstimatedMg = m;
        } else {
            mEstimatedMg.copyFrom(m);
        }

        for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
            mEstimatedMg.setElementAt(i, i,
                    mEstimatedMg.getElementAt(i, i) - 1.0);
        }

        if (mEstimatedGg == null) {
            mEstimatedGg = new Matrix(
                    BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            mEstimatedGg.initialize(0.0);
        }

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Computes gravity versor error at the end of a sequence using provided
     * parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param i      row position.
     * @param params array containing parameters for the general purpose case
     *               when G-dependent cross biases are taken into account. Must
     *               have length 18.
     * @return error between estimated and measured gravity versor.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneralWithGDependentCrossBiases(
            final int i, final double[] params)
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3142
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4122
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final double[] initial = new double[GENERAL_UNKNOWNS];

                initial[0] = mInitialSx;
                initial[1] = mInitialSy;
                initial[2] = mInitialSz;

                initial[3] = mInitialMxy;
                initial[4] = mInitialMxz;
                initial[5] = mInitialMyx;
                initial[6] = mInitialMyz;
                initial[7] = mInitialMzx;
                initial[8] = mInitialMzy;

                return initial;

            }

            @Override
            public void evaluate(
                    final int i, final double[] point, final double[] result,
                    final double[] params, final Matrix jacobian) {
                // We know that:
                //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                //  fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
                //  fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters sx, sy, sz,
                // mxy, mxz, myx, myz, mzx and mzy is:

                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myx) = 0.0
                // d(fmeasx)/d(myz) = 0.0
                // d(fmeasx)/d(mzx) = 0.0
                // d(fmeasx)/d(mzy) = 0.0

                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myx) = ftruex
                // d(fmeasy)/d(myz) = ftruez
                // d(fmeasy)/d(mzx) = 0.0
                // d(fmeasy)/d(mzy) = 0.0

                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myx) = 0.0
                // d(fmeasz)/d(myz) = 0.0
                // d(fmeasz)/d(mzx) = ftruex
                // d(fmeasz)/d(mzy) = ftruey

                final double sx = params[0];
                final double sy = params[1];
                final double sz = params[2];

                final double mxy = params[3];
                final double mxz = params[4];
                final double myx = params[5];
                final double myz = params[6];
                final double mzx = params[7];
                final double mzy = params[8];

                final double ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3646
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4317
                initial[3] = mInitialSx;
                initial[4] = mInitialSy;
                initial[5] = mInitialSz;

                initial[6] = mInitialMxy;
                initial[7] = mInitialMxz;
                initial[8] = mInitialMyx;
                initial[9] = mInitialMyz;
                initial[10] = mInitialMzx;
                initial[11] = mInitialMzy;

                return initial;
            }

            @Override
            public void evaluate(final int i, final double[] point,
                                 final double[] result, final double[] params,
                                 final Matrix jacobian) {
                // We know that:
                //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                //  fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
                //  fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
                // sz, mxy, mxz, myx, myz, mzx and mzy is:

                // d(fmeasx)/d(bx) = 1.0
                // d(fmeasx)/d(by) = 0.0
                // d(fmeasx)/d(bz) = 0.0
                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myx) = 0.0
                // d(fmeasx)/d(myz) = 0.0
                // d(fmeasx)/d(mzx) = 0.0
                // d(fmeasx)/d(mzy) = 0.0

                // d(fmeasy)/d(bx) = 0.0
                // d(fmeasy)/d(by) = 1.0
                // d(fmeasy)/d(bz) = 0.0
                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myx) = ftruex
                // d(fmeasy)/d(myz) = ftruez
                // d(fmeasy)/d(mzx) = 0.0
                // d(fmeasy)/d(mzy) = 0.0

                // d(fmeasz)/d(bx) = 0.0
                // d(fmeasz)/d(by) = 0.0
                // d(fmeasz)/d(bz) = 1.0
                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myx) = 0.0
                // d(fmeasz)/d(myz) = 0.0
                // d(fmeasz)/d(mzx) = ftruex
                // d(fmeasz)/d(mzy) = ftruey

                final double bx = params[0];
                final double by = params[1];
                final double bz = params[2];

                final double sx = params[3];
                final double sy = params[4];
                final double sz = params[5];

                final double mxy = params[6];
                final double mxz = params[7];
                final double myx = params[8];
                final double myz = params[9];
                final double mzx = params[10];
                final double mzy = params[11];

                final double ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8405
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9816
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10020
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8828
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10321
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10540
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
File Line
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 404
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator3D.java 404
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator2D.java 415
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 414
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this estimator is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }


    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1292
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1403
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double fMeasX = measuredKinematics.getFx();
            final double fMeasY = measuredKinematics.getFy();
            final double fMeasZ = measuredKinematics.getFz();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, fTrueX);
            a.setElementAt(i, 1, 0.0);
            a.setElementAt(i, 2, 0.0);
            a.setElementAt(i, 3, fTrueY);
            a.setElementAt(i, 4, fTrueZ);
            a.setElementAt(i, 5, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 12081
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 12677
            mEstimatedMg = preliminaryResult.mEstimatedMg;
            mEstimatedGg = preliminaryResult.mEstimatedGg;
        }
    }

    /**
     * Converts provided NED position expressed in terms of latitude, longitude and height respect
     * mean Earth surface, to position expressed in ECEF coordinates.
     *
     * @param position NED position to be converted.
     * @return converted position expressed in ECEF coordinates.
     */
    private static ECEFPosition convertPosition(final NEDPosition position) {
        final ECEFVelocity velocity = new ECEFVelocity();
        final ECEFPosition result = new ECEFPosition();
        NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                position.getLatitude(), position.getLongitude(), position.getHeight(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(),
                acceleration.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second.
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value.
     */
    private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
        return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(),
                angularSpeed.getUnit(), AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Converts time instance to seconds.
     *
     * @param time time instance to be converted.
     * @return converted value.
     */
    private static double convertTime(final Time time) {
        return TimeConverter.convert(time.getValue().doubleValue(),
                time.getUnit(), TimeUnit.SECOND);
    }

    /**
     * Internal class containing estimated preliminary result.
     */
    protected static class PreliminaryResult {
        /**
         * Estimated gyroscope scale factors and cross coupling errors.
         * This is the product of matrix Tg containing cross coupling errors and Kg
         * containing scaling factors.
         * So that:
         * <pre>
         *     Mg = [sx    mxy  mxz] = Tg*Kg
         *          [myx   sy   myz]
         *          [mzx   mzy  sz ]
         * </pre>
         * Where:
         * <pre>
         *     Kg = [sx 0   0 ]
         *          [0  sy  0 ]
         *          [0  0   sz]
         * </pre>
         * and
         * <pre>
         *     Tg = [1          -alphaXy    alphaXz ]
         *          [alphaYx    1           -alphaYz]
         *          [-alphaZx   alphaZy     1       ]
         * </pre>
         * Hence:
         * <pre>
         *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
         *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
         *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
         * </pre>
         * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
         * are considered to be zero if the gyroscope z-axis is assumed to be the same
         * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
         * becomes upper diagonal:
         * <pre>
         *     Mg = [sx    mxy  mxz]
         *          [0     sy   myz]
         *          [0     0    sz ]
         * </pre>
         * Values of this matrix are unitless.
         */
        private Matrix mEstimatedMg;
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator2D.java 333
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator3D.java 333
            final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Robustly estimates position for a radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4881
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6203
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5119
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6501
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4223
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4042
        if (mEstimatedMm == null) {
            mEstimatedMm = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
        } else {
            mEstimatedMm.initialize(0.0);
        }

        mEstimatedMm.setElementAt(0, 0, sx);

        mEstimatedMm.setElementAt(0, 1, mxy);
        mEstimatedMm.setElementAt(1, 1, sy);

        mEstimatedMm.setElementAt(0, 2, mxz);
        mEstimatedMm.setElementAt(1, 2, myz);
        mEstimatedMm.setElementAt(2, 2, sz);

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     * @throws IOException                              if world magnetic model cannot be loaded.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException, IOException {
        // The magnetometer model is:
        // mBmeas = ba + (I + Mm) * mBtrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // mBmeas = ba + (I + Mm) * mBtrue

        // Hence:
        //  [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        //  [mBmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [mBtruey]
        //  [mBmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [mBtruez]

        //  [mBmeasx] = [bx] +   [1+sx   mxy     mxz ][mBtruex]
        //  [mBmeasy]   [by]     [myx    1+sy    myz ][mBtruey]
        //  [mBmeasz]   [bz]     [mzx    mzy     1+sz][mBtruez]

        //  mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy = by + myx * mBtruex + (1+sy) * mBtruey + myz * mBtruez
        //  mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + (1+sz) * mBtruez

        // Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
        // Reordering:
        //  mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy = by + myx * mBtruex + mBtruey + sy * mBtruey + myz * mBtruez
        //  mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + mBtruez + sz * mBtruez

        //  mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy - mBtruey = by + myx * mBtruex + sy * mBtruey + myz * mBtruez
        //  mBmeasz - mBtruez = bz + mzx * mBtruex + mzy * mBtruey + sz * mBtruez

        // [1   0   0   mBtruex  0        0        mBtruey  mBtruez  0        0        0        0      ][bx ] = [mBmeasx - mBtruex]
        // [0   1   0   0        mBtruey  0        0        0        mBtruex  mBtruez  0        0      ][by ]   [mBmeasy - mBtruey]
        // [0   0   1   0        0        mBtruez  0        0        0        0        mBtruex  mBtruey][bz ]   [mBmeasz - mBtruez]
        //                                                                                              [sx ]
        //                                                                                              [sy ]
        //                                                                                              [sz ]
        //                                                                                              [mxy]
        //                                                                                              [mxz]
        //                                                                                              [myx]
        //                                                                                              [myz]
        //                                                                                              [mzx]
        //                                                                                              [mzy]

        mFitter.setFunctionEvaluator(
                new LevenbergMarquardtMultiVariateFunctionEvaluator() {
                    @Override
                    public int getNumberOfDimensions() {
                        // Input points are true magnetic flux density coordinates
                        return BodyMagneticFluxDensity.COMPONENTS;
                    }

                    @Override
                    public int getNumberOfVariables() {
                        // The multivariate function returns the components of measured magnetic flux density
                        return BodyMagneticFluxDensity.COMPONENTS;
                    }

                    @Override
                    public double[] createInitialParametersArray() {
                        final double[] initial = new double[GENERAL_UNKNOWNS];

                        initial[0] = mInitialHardIronX;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6538
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6977
            final KnownBiasAndPositionAccelerometerCalibrationListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    public int getMinimumRequiredMeasurements() {
        return mCommonAxisUsed ? MINIMUM_MEASUREMENTS_COMON_Z_AXIS :
                MINIMUM_MEASUREMENTS_GENERAL;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= getMinimumRequiredMeasurements()
                && mPosition != null;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException
                | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1869
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 536
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1888
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1771
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 4033
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 544
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 4096
                                    PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum required
     *                                  number of samples (10 or 13).
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinimumRequiredMeasurements()) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5655
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5850
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6977
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7167
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5931
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6133
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7318
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7525
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 926
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2156
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyMagneticFluxDensity measurement : mMeasurements) {
            final BodyMagneticFluxDensity measuredMagneticFluxDensity =
                    measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final ECEFFrame ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final double year = measurement.getYear();

            final double latitude = nedFrame.getLatitude();
            final double longitude = nedFrame.getLongitude();
            final double height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body mangetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb,
                    expectedMagneticFluxDensity);

            final double bMeasX = measuredMagneticFluxDensity.getBx();
            final double bMeasY = measuredMagneticFluxDensity.getBy();
            final double bMeasZ = measuredMagneticFluxDensity.getBz();

            final double bTrueX = expectedMagneticFluxDensity.getBx();
            final double bTrueY = expectedMagneticFluxDensity.getBy();
            final double bTrueZ = expectedMagneticFluxDensity.getBz();

            a.setElementAt(i, 0, 1.0);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1067
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2014
        final Matrix a = new Matrix(rows, GENERAL_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyMagneticFluxDensity measurement : mMeasurements) {
            final BodyMagneticFluxDensity measuredMagneticFluxDensity =
                    measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final ECEFFrame ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final double year = measurement.getYear();

            final double latitude = nedFrame.getLatitude();
            final double longitude = nedFrame.getLongitude();
            final double height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body mangetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb,
                    expectedMagneticFluxDensity);

            final double bMeasX = measuredMagneticFluxDensity.getBx();
            final double bMeasY = measuredMagneticFluxDensity.getBy();
            final double bMeasZ = measuredMagneticFluxDensity.getBz();

            final double bTrueX = expectedMagneticFluxDensity.getBx();
            final double bTrueY = expectedMagneticFluxDensity.getBy();
            final double bTrueZ = expectedMagneticFluxDensity.getBz();

            a.setElementAt(i, 0, 1.0);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3064
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3554
        if (mEstimatedMa == null) {
            mEstimatedMa = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
        } else {
            mEstimatedMa.initialize(0.0);
        }

        mEstimatedMa.setElementAt(0, 0, sx);

        mEstimatedMa.setElementAt(0, 1, mxy);
        mEstimatedMa.setElementAt(1, 1, sy);

        mEstimatedMa.setElementAt(0, 2, mxz);
        mEstimatedMa.setElementAt(1, 2, myz);
        mEstimatedMa.setElementAt(2, 2, sz);

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // Hence:
        //  [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        //  [fmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [ftruey]
        //  [fmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [ftruez]

        //  [fmeasx] = [bx] +   [1+sx   mxy     mxz ][ftruex]
        //  [fmeasy]   [by]     [myx    1+sy    myz ][ftruey]
        //  [fmeasz]   [bz]     [mzx    mzy     1+sz][ftruez]

        //  fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + myx * ftruex + (1+sy) * ftruey + myz * ftruez
        //  fmeasz = bz + mzx * ftruex + mzy * ftruey + (1+sz) * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
        // Reordering:
        //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
        //  fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

        //  fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy - ftruey - by = myx * ftruex + sy * ftruey + myz * ftruez
        //  fmeasz - ftruez - bz = mzx * ftruex + mzy * ftruey + sz * ftruez

        // [ftruex  0       0       ftruey  ftruez  0       0       0       0     ][sx ] = [fmeasx - ftruex - bx]
        // [0       ftruey  0       0       0       ftruex  ftruez  0       0     ][sy ]   [fmeasy - ftruey - by]
        // [0       0       ftruez  0       0       0       0       ftruex  ftruey][sz ]   [fmeasz - ftruez - bz]
        //                                                                         [mxy]
        //                                                                         [mxz]
        //                                                                         [myx]
        //                                                                         [myz]
        //                                                                         [mzx]
        //                                                                         [mzy]

        mFitter.setFunctionEvaluator(new LevenbergMarquardtMultiVariateFunctionEvaluator() {
            @Override
            public int getNumberOfDimensions() {
                // Input points are true specific force coordinates
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public int getNumberOfVariables() {
                // The multivariate function returns the components of measured specific force
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final double[] initial = new double[GENERAL_UNKNOWNS];

                initial[0] = mInitialSx;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4968
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5128
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5374
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6696
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6290
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6450
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5208
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5371
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5631
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7013
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6590
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6753
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 510
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3648
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3605
            final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Indicates whether calibrator is ready to start the estimator.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null
                && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or no.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Gets Earth's magnetic model.
     *
     * @return Earth's magnetic model or null if not provided.
     */
    @Override
    public WorldMagneticModel getMagneticModel() {
        return mMagneticModel;
    }

    /**
     * Sets Earth's magnetic model.
     * If not provided a default model will be loaded internally.
     *
     * @param magneticModel Earth's magnetic model to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMagneticModel(final WorldMagneticModel magneticModel)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMagneticModel = magneticModel;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | IOException e) {
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 1229
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 629
                                               final ECEFFrame oldFrame) {
        return estimateKinematicsAndReturnNew(timeInterval, frame, oldFrame);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(final double timeInterval,
                                               final CoordinateTransformation c,
                                               final CoordinateTransformation oldC,
                                               final Speed vx, final Speed vy, final Speed vz,
                                               final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                               final double x, final double y, final double z) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz,
                oldVx, oldVy, oldVz, x, y, z);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(final Time timeInterval,
                                               final CoordinateTransformation c,
                                               final CoordinateTransformation oldC,
                                               final Speed vx, final Speed vy, final Speed vz,
                                               final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                               final double x, final double y, final double z) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz,
                oldVx, oldVy, oldVz, x, y, z);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param position     cartesian body position resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(final double timeInterval,
                                               final CoordinateTransformation c,
File Line
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator2D.java 174
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator3D.java 174
            final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on ranging distances.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on ranging distances.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this estimator is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }


    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position for a radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 240
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 785
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1908
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 247
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 1939
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 228
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1005
    public void setStopThreshold(double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return LMedSRobustKnownFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/indoor/position/LinearMixedPositionEstimator.java 79
com/irurueta/navigation/indoor/position/LinearRangingAndRssiPositionEstimator.java 80
com/irurueta/navigation/indoor/position/LinearRangingPositionEstimator.java 79
com/irurueta/navigation/indoor/position/LinearRssiPositionEstimator.java 82
    public LinearMixedPositionEstimator(final MixedPositionEstimatorListener<P> listener) {
        super(listener);
        init();
    }

    /**
     * Indicates whether an homogeneous linear solver is used to estimate position.
     *
     * @return true if homogeneous linear solver is used, false if an inhomogeneous
     * linear one is used instead.
     */
    public boolean isHomogeneousLinearSolverUsed() {
        return mUseHomogeneousLinearSolver;
    }

    /**
     * Specifies whether an homogeneous linear solver is used to estimate position.
     *
     * @param useHomogeneousLinearSolver true if homogeneous linear solver is used, false
     *                                   if an inhomogeneous linear one is used instead.
     * @throws LockedException if estimator is locked.
     */
    public void setHomogeneousLinearSolverUsed(final boolean useHomogeneousLinearSolver)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        mUseHomogeneousLinearSolver = useHomogeneousLinearSolver;
    }

    /**
     * Gets minimum required number of located radio sources to perform lateration.
     *
     * @return minimum required number of located radio sources to perform lateration.
     */
    @Override
    public int getMinRequiredSources() {
        return mUseHomogeneousLinearSolver ?
                mHomogeneousTrilaterationSolver.getMinRequiredPositionsAndDistances() :
                mInhomogeneousTrilaterationSolver.getMinRequiredPositionsAndDistances();
    }

    /**
     * Indicates whether estimator is ready to find a solution.
     *
     * @return true if estimator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return (!mUseHomogeneousLinearSolver && mInhomogeneousTrilaterationSolver.isReady()) ||
                (mUseHomogeneousLinearSolver && mHomogeneousTrilaterationSolver.isReady());
    }

    /**
     * Returns boolean indicating whether this estimator is locked because an estimation is already in progress.
     *
     * @return true if estimator is locked, false otherwise.
     */
    @Override
    public boolean isLocked() {
        return mInhomogeneousTrilaterationSolver.isLocked() ||
                mHomogeneousTrilaterationSolver.isLocked();
    }

    /**
     * Estimates position based on provided located radio sources and RSSI readings of
     * such radio sources at an unknown location.
     *
     * @throws LockedException             if estimator is locked.
     * @throws NotReadyException           if estimator is not ready.
     * @throws PositionEstimationException if estimation fails for some other reason.
     */
    @SuppressWarnings("Duplicates")
    @Override
    public void estimate() throws LockedException, NotReadyException,
            PositionEstimationException {
        try {
            if (mUseHomogeneousLinearSolver) {
                mHomogeneousTrilaterationSolver.solve();
                mEstimatedPositionCoordinates =
                        mHomogeneousTrilaterationSolver.getEstimatedPositionCoordinates();
            } else {
                mInhomogeneousTrilaterationSolver.solve();
                mEstimatedPositionCoordinates =
                        mInhomogeneousTrilaterationSolver.getEstimatedPositionCoordinates();
            }
        } catch (LaterationException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 7067
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4908
                        mFmeasZ = point[2];

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateCommonAxis(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double m11 = result[0];

        final double m12 = result[1];
        final double m22 = result[2];

        final double m13 = result[3];
        final double m23 = result[4];
        final double m33 = result[5];

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, 0.0);
        m.setElementAtIndex(2, 0.0);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        setResult(m);
    }

    /**
     * Makes proper conversion of internal cross-coupling and bias matrices.
     *
     * @param m internal cross-coupling matrix.
     */
    private void setResult(final Matrix m) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3038
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4495
        mB = invInitialM.multiplyAndReturnNew(bg);

        mFitter.setFunctionEvaluator(
                new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
                    @Override
                    public int getNumberOfDimensions() {
                        // Before and after normalized gravity versors
                        return 2 * BodyKinematics.COMPONENTS;
                    }

                    @Override
                    public double[] createInitialParametersArray() {
                        final double[] initial =
                                new double[COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES];

                        // upper diagonal cross coupling errors M
                        int k = 0;
                        for (int j = 0; j < BodyKinematics.COMPONENTS; j++) {
                            for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        // g-dependent cross biases G
                        final int num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                        for (int i = 0, j = k; i < num; i++, j++) {
                            initial[j] = initialG.getElementAtIndex(i);
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4677
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4812
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5041
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6363
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5205
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6527
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5211
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5380
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5999
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6134
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(position,
                        turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6533
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6702
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4897
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5044
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5282
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6664
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5449
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6831
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by
     *                                      this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5455
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5637
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by
     *                                      this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6284
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6426
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6837
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7019
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3412
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4912
                        return evaluateCommonAxis(i, params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double m11 = result[0];

        final double m12 = result[1];
        final double m22 = result[2];

        final double m13 = result[3];
        final double m23 = result[4];
        final double m33 = result[5];

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, 0.0);
        m.setElementAtIndex(2, 0.0);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        setResult(m);
    }

    /**
     * Internal method to perform general calibration when G-dependant cross
     * biases are ignored.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneral()
            throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4964
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5103
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4758
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4897
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8305
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9919
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8505
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9713
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must be
     *                                      3x1 and is expressed in radians
     *                                      per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8719
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10430
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8934
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10209
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.*
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4027
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5650
            final int i, final double[] params)
            throws EvaluationException {

        final double bx = params[0];
        final double by = params[1];
        final double bz = params[2];

        final double m11 = params[3];
        final double m21 = params[4];
        final double m31 = params[5];

        final double m12 = params[6];
        final double m22 = params[7];
        final double m32 = params[8];

        final double m13 = params[9];
        final double m23 = params[10];
        final double m33 = params[11];

        final double g11 = params[12];
        final double g21 = params[13];
        final double g31 = params[14];

        final double g12 = params[15];
        final double g22 = params[16];
        final double g32 = params[17];

        final double g13 = params[18];
        final double g23 = params[19];
        final double g33 = params[20];

        return evaluate(i, bx, by, bz, m11, m21, m31, m12, m22, m32,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 828
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 813
                            mListener.onCalibrateProgressChange(RANSACRobustEasyGyroscopeCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            setupAccelerationFixer();

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7497
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8907
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg, initialGg,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg, initialGg,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7585
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7759
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8993
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9164
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7874
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9361
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7966
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8143
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9453
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9630
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised
     *                              by this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingRadioSourceEstimator2D.java 816
com/irurueta/navigation/indoor/radiosource/RobustRangingRadioSourceEstimator3D.java 819
        final Point2D initialPosition = result.getEstimatedPosition();

        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mReadings.size();

            mInnerReadings.clear();

            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    //sample is inlier
                    mInnerReadings.add(mReadings.get(i));
                }
            }

            try {
                mInnerEstimator.setInitialPosition(initialPosition);
                mInnerEstimator.setReadings(mInnerReadings);

                mInnerEstimator.setNonLinearSolverEnabled(true);
                mInnerEstimator.setUseReadingPositionCovariances(
                        mUseReadingPositionCovariances);
                mInnerEstimator.estimate();

                final Matrix cov = mInnerEstimator.getEstimatedCovariance();
                if (mKeepCovariance && cov != null) {
                    //keep covariance
                    mEstimatedPositionCovariance = mCovariance = cov;

                } else {
                    mCovariance = null;
                    mEstimatedPositionCovariance = null;
                }

                mEstimatedPosition = mInnerEstimator.getEstimatedPosition();
            } catch (final Exception e) {
                //refinement failed, so we return input value, and covariance
                //becomes unavailable
                mCovariance = null;
                mEstimatedPositionCovariance = null;

                mEstimatedPosition = initialPosition;
            }
        } else {
            mCovariance = null;
            mEstimatedPositionCovariance = null;

            mEstimatedPosition = initialPosition;
        }
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 352
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 358
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 358
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 358
            final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<PreliminaryResult> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8021
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9612
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8204
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9429
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8419
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10102
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8612
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9907
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 1756
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1015
                final double specificForceZ = invAveCbe.getElementAtIndex(2);

                // save result data
                result.setSpecificForceCoordinates(specificForceX, specificForceY,
                        specificForceZ);
                result.setAngularRateCoordinates(angularRateX, angularRateY,
                        angularRateZ);

            } catch (final AlgebraException ignore) {
                // never happens
            }
        } else {
            // If time interval is zero, set angular rate and specific force to zero
            result.setSpecificForceCoordinates(0.0, 0.0, 0.0);
            result.setAngularRateCoordinates(0.0, 0.0, 0.0);
        }
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final Time timeInterval,
                                          final CoordinateTransformation c,
                                          final CoordinateTransformation oldC,
                                          final double vx, final double vy, final double vz,
                                          final double oldVx, final double oldVy, final double oldVz,
                                          final double x, final double y, final double z,
                                          final BodyKinematics result) {
        estimateKinematics(TimeConverter.convert(timeInterval.getValue().doubleValue(),
                timeInterval.getUnit(), TimeUnit.SECOND),
                c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final double timeInterval,
                                          final CoordinateTransformation c,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3340
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3117
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4578
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4828
        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, 0.0);
        m.setElementAtIndex(2, 0.0);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        final Matrix g = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        g.setElementAtIndex(0, g11);
        g.setElementAtIndex(1, g21);
        g.setElementAtIndex(2, g31);

        g.setElementAtIndex(3, g12);
        g.setElementAtIndex(4, g22);
        g.setElementAtIndex(5, g32);

        g.setElementAtIndex(6, g13);
        g.setElementAtIndex(7, g23);
        g.setElementAtIndex(8, g33);

        setResult(m, b, g);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3511
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3275
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4766
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5032
        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, m21);
        m.setElementAtIndex(2, m31);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, m32);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        final Matrix g = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        g.setElementAtIndex(0, g11);
        g.setElementAtIndex(1, g21);
        g.setElementAtIndex(2, g31);

        g.setElementAtIndex(3, g12);
        g.setElementAtIndex(4, g22);
        g.setElementAtIndex(5, g32);

        g.setElementAtIndex(6, g13);
        g.setElementAtIndex(7, g23);
        g.setElementAtIndex(8, g33);

        setResult(m, b, g);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7352
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8760
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7715
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9205
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/indoor/fingerprint/FirstOrderNonLinearFingerprintPositionEstimator3D.java 157
com/irurueta/navigation/indoor/fingerprint/SecondOrderNonLinearFingerprintPositionEstimator3D.java 157
com/irurueta/navigation/indoor/fingerprint/ThirdOrderNonLinearFingerprintPositionEstimator3D.java 154
    }

    /**
     * Evaluates a non-linear multi dimension function at provided point using
     * provided parameters and returns its evaluation and derivatives of the
     * function respect the function parameters.
     *
     * @param i           number of sample being evaluated.
     * @param point       point where function will be evaluated.
     * @param params      initial parameters estimation to be tried. These will
     *                    change as the Levenberg-Marquard algorithm iterates to the best solution.
     *                    These are used as input parameters along with point to evaluate function.
     * @param derivatives partial derivatives of the function respect to each
     *                    provided parameter.
     * @return function evaluation at provided point.
     */
    @Override
    @SuppressWarnings("Duplicates")
    protected double evaluate(
            final int i, final double[] point, final double[] params,
            final double[] derivatives) {
        //This method implements received power at point pi = (xi, yi, zi) and its derivatives

        //Pr(pi) = Pr(p1)
        //  - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
        //  - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
        //  - 10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1)

        final double xi = params[0];
        final double yi = params[1];
        final double zi = params[2];

        //received power
        final double pr = point[0];

        //fingerprint coordinates
        final double x1 = point[1];
        final double y1 = point[2];
        final double z1 = point[3];

        //radio source coordinates
        final double xa = point[4];
        final double ya = point[5];
        final double za = point[6];

        //path loss exponent
        final double n = point[7];

        final double ln10 = Math.log(10.0);

        final double diffXi1 = xi - x1;
        final double diffYi1 = yi - y1;
        final double diffZi1 = zi - z1;

        final double diffX1a = x1 - xa;
        final double diffY1a = y1 - ya;
        final double diffZ1a = z1 - za;

        final double diffX1a2 = diffX1a * diffX1a;
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator2D.java 377
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingRadioSourceEstimator2D.java 154
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator2D.java 389
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if robust estimator is locked because an
     *                                  estimation is already in progress.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<Solution<Point2D>> innerEstimator =
                new MSACRobustEstimator<>(
                        new MSACRobustEstimatorListener<Solution<Point2D>>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator3D.java 377
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingRadioSourceEstimator3D.java 152
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator3D.java 391
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if robust estimator is locked because an
     *                                  estimation is already in progress.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<Solution<Point3D>> innerEstimator =
                new MSACRobustEstimator<>(
                        new MSACRobustEstimatorListener<Solution<Point3D>>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator2D.java 840
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator3D.java 334
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator2D.java 852
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator3D.java 838
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator2D.java 334
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator3D.java 852
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Solution<Point3D>> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1577
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1585
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3269
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3055
                        for (int j = 0; j < BodyKinematics.COMPONENTS; j++) {
                            for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        // g-dependent cross biases G
                        final int num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                        for (int i = 0, j = k; i < num; i++, j++) {
                            initial[j] = initialG.getElementAtIndex(i);
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {
                        mI = i;

                        // point contains fixed gravity versor values for current
                        // sequence
                        mPoint = point;

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateCommonAxisWithGDependentCrossBiases(i, params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4513
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4754
                        for (int j = 0; j < BodyKinematics.COMPONENTS; j++) {
                            for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        // g-dependent cross biases G
                        final int num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                        for (int i = 0, j = k; i < num; i++, j++) {
                            initial[j] = initialG.getElementAtIndex(i);
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {

                        mMeasAngularRateX = point[0];
                        mMeasAngularRateY = point[1];
                        mMeasAngularRateZ = point[2];

                        mFmeasX = point[3];
                        mFmeasY = point[4];
                        mFmeasZ = point[5];

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateCommonAxisWitGDependentCrossBiases(params);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1558
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1528
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 828
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 813
                                    PROSACRobustEasyGyroscopeCalibrator.this, progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            setupAccelerationFixer();

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 565
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 356
                                            PROSACRobustKnownFrameMagnetometerCalibrator.this,
                                            progress);
                                }
                            }
                        });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;

            setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 566
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 356
                                    PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;

            setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2271
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1117
                                    PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;

            initialize();

            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2269
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1119
                                    PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;

            initialize();

            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 976
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 355
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 987
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 899
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 2023
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 362
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 2053
                                            RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.this,
                                            progress);
                                }
                            }
                        });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7671
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9250
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7842
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9076
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8054
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9718
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8231
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9541
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 15723
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 7485
    private static double convertTimeToDouble(final Time time) {
        return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(),
                TimeUnit.SECOND);
    }

    /**
     * Converts provided distance instance into its corresponding value expressed in
     * meters.
     *
     * @param distance distance instance to be converted.
     * @return converted value expressed in meters.
     */
    private static double convertDistanceToDouble(final Distance distance) {
        return DistanceConverter.convert(distance.getValue().doubleValue(),
                distance.getUnit(), DistanceUnit.METER);
    }

    /**
     * Converts provided speed instance into its corresponding value expressed in
     * meters per second.
     *
     * @param speed speed instance to be converted.
     * @return converted value expressed in meters per second.
     */
    private static double convertSpeedToDouble(final Speed speed) {
        return SpeedConverter.convert(speed.getValue().doubleValue(),
                speed.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Converts provided acceleration instance into its corresponding value expressed
     * in meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value expressed in meters per squared second.
     */
    private static double convertAccelerationToDouble(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(),
                acceleration.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts provided angular speed into its corresponding value expressed in
     * radians per second.
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value expressed in radians per second.
     */
    private static double convertAngularSpeedToDouble(final AngularSpeed angularSpeed) {
        return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(),
                angularSpeed.getUnit(), AngularSpeedUnit.RADIANS_PER_SECOND);
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 406
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator3D.java 175
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator2D.java 417
                initialPathLossExponent, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this estimator is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }


    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator3D.java 406
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator2D.java 175
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 416
                initialPathLossExponent, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on received power (RSSI) expressed
     * in dBm's between received value that should have been received on estimated
     * isotropical model and actual measured value.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this estimator is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }


    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Solution<Point3D>> innerEstimator =
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 2068
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 2346
    public boolean equals(final INSLooselyCoupledKalmanState other,
                          final double threshold) {
        if (other == null) {
            return false;
        }

        return Math.abs(mVx - other.mVx) <= threshold
                && Math.abs(mVy - other.mVy) <= threshold
                && Math.abs(mVz - other.mVz) <= threshold
                && Math.abs(mX - other.mX) <= threshold
                && Math.abs(mY - other.mY) <= threshold
                && Math.abs(mZ - other.mZ) <= threshold
                && Math.abs(mAccelerationBiasX - other.mAccelerationBiasX) <= threshold
                && Math.abs(mAccelerationBiasY - other.mAccelerationBiasY) <= threshold
                && Math.abs(mAccelerationBiasZ - other.mAccelerationBiasZ) <= threshold
                && Math.abs(mGyroBiasX - other.mGyroBiasX) <= threshold
                && Math.abs(mGyroBiasY - other.mGyroBiasY) <= threshold
                && Math.abs(mGyroBiasZ - other.mGyroBiasZ) <= threshold
                && other.mBodyToEcefCoordinateTransformationMatrix != null &&
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2660
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2927
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3186
            final KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException |
                com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 727
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 732
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/indoor/position/RANSACRobustMixedPositionEstimator2D.java 147
com/irurueta/navigation/indoor/position/RANSACRobustRangingAndRssiPositionEstimator2D.java 147
com/irurueta/navigation/indoor/position/RANSACRobustRangingPositionEstimator2D.java 147
com/irurueta/navigation/indoor/position/RANSACRobustRssiPositionEstimator2D.java 145
            final RobustMixedPositionEstimatorListener<Point2D> listener) {
        super(listener);
        init();
        internalSetSources(sources);
        internalSetFingerprint(fingerprint);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing
     * possible solutions.
     * The threshold refers to the amount of error on distance between estimated
     * position and distances provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return ((RANSACRobustLateration2DSolver) mLaterationSolver).
                getThreshold();
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing
     * possible solutions.
     * The threshold refers to the amount of error on distance between estimated position
     * and distances provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this estimator is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        ((RANSACRobustLateration2DSolver) mLaterationSolver).
                setThreshold(threshold);
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return ((RANSACRobustLateration2DSolver) mLaterationSolver).
                isComputeAndKeepInliersEnabled();
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not
     *                              kept.
     * @throws LockedException if this estimator is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        ((RANSACRobustLateration2DSolver) mLaterationSolver).
                setComputeAndKeepInliersEnabled(computeAndKeepInliers);
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return ((RANSACRobustLateration2DSolver) mLaterationSolver).
                isComputeAndKeepResiduals();
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this estimator is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        ((RANSACRobustLateration2DSolver) mLaterationSolver).
                setComputeAndKeepResidualsEnabled(computeAndKeepResiduals);
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }

    /**
     * Initializes robust lateration solver.
     */
    private void init() {
        mLaterationSolver = new RANSACRobustLateration2DSolver(
                mTrilaterationSolverListener);
    }
}
File Line
com/irurueta/navigation/indoor/position/RANSACRobustMixedPositionEstimator3D.java 147
com/irurueta/navigation/indoor/position/RANSACRobustRangingAndRssiPositionEstimator3D.java 147
com/irurueta/navigation/indoor/position/RANSACRobustRangingPositionEstimator3D.java 146
com/irurueta/navigation/indoor/position/RANSACRobustRssiPositionEstimator3D.java 146
            final RobustMixedPositionEstimatorListener<Point3D> listener) {
        super(listener);
        init();
        internalSetSources(sources);
        internalSetFingerprint(fingerprint);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing
     * possible solutions.
     * The threshold refers to the amount of error on distance between estimated
     * position and distances provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return ((RANSACRobustLateration3DSolver) mLaterationSolver).
                getThreshold();
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing
     * possible solutions.
     * The threshold refers to the amount of error on distance between estimated position
     * and distances provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this estimator is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        ((RANSACRobustLateration3DSolver) mLaterationSolver).
                setThreshold(threshold);
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return ((RANSACRobustLateration3DSolver) mLaterationSolver).
                isComputeAndKeepInliersEnabled();
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not
     *                              kept.
     * @throws LockedException if this estimator is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        ((RANSACRobustLateration3DSolver) mLaterationSolver).
                setComputeAndKeepInliersEnabled(computeAndKeepInliers);
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return ((RANSACRobustLateration3DSolver) mLaterationSolver).
                isComputeAndKeepResiduals();
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this estimator is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        ((RANSACRobustLateration3DSolver) mLaterationSolver).
                setComputeAndKeepResidualsEnabled(computeAndKeepResiduals);
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }

    /**
     * Initializes robust lateration solver.
     */
    private void init() {
        mLaterationSolver = new RANSACRobustLateration3DSolver(
                mTrilaterationSolverListener);
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator.java 2421
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator.java 2290
            final List<? extends ReadingLocated<P>> readings) {
        if (!areValidReadings(readings)) {
            throw new IllegalArgumentException();
        }

        mReadings = readings;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than required minimum.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinReadings()) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }

    /**
     * Creates a ranging reading from a ranging and RSSI reading.
     *
     * @param reading input reading to convert from.
     * @return a ranging reading containing only the ranging data of input reading.
     */
    private RangingReadingLocated<S, P> createRangingReading(
            final RangingAndRssiReadingLocated<S, P> reading) {
        return new RangingReadingLocated<>(reading.getSource(),
                reading.getDistance(), reading.getPosition(),
                reading.getDistanceStandardDeviation(),
                reading.getPositionCovariance());
    }

    /**
     * Creates an RSSI reading from a ranging and RSSI reading.
     *
     * @param reading input reading to convert from.
     * @return an RSSI reading containing only the RSSI data of input reading.
     */
    private RssiReadingLocated<S, P> createRssiReading(
            final RangingAndRssiReadingLocated<S, P> reading) {
        return new RssiReadingLocated<>(reading.getSource(),
                reading.getRssi(), reading.getPosition(),
                reading.getRssiStandardDeviation(),
                reading.getPositionCovariance());
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1577
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 353
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 359
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 359
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5529
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5877
                mFtrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (mTmp == null) {
                mTmp = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            mMeasAngularRate.setElementAtIndex(0, mMeasAngularRateX);
            mMeasAngularRate.setElementAtIndex(1, mMeasAngularRateY);
            mMeasAngularRate.setElementAtIndex(2, mMeasAngularRateZ);

            mFmeas.setElementAtIndex(0, mFmeasX);
            mFmeas.setElementAtIndex(1, mFmeasY);
            mFmeas.setElementAtIndex(2, mFmeasZ);

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);

            mB.setElementAtIndex(0, mBiasX);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1585
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 359
        super(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<PreliminaryResult> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2064
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2061
        super(position, measurements, commonAxisUsed, hardIron,
                initialMm, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates magnetometer calibration parameters containing soft-iron
     * scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1784
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 888
                                    PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1889
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 561
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1912
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 976
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 355
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 987
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1791
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 4055
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 568
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 4119
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 899
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 2023
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 362
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 2053
                                    PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.this, progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 189
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 195
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 191
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 192
            final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2762
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2934
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 7253
        }
    }

    /**
     * Converts a time instance expressed in milliseconds since epoch time
     * (January 1st, 1970 at midnight) to a decimal year.
     *
     * @param timestampMillis milliseconds value to be converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final Long timestampMillis) {
        if (timestampMillis == null) {
            return null;
        }

        final GregorianCalendar calendar = new GregorianCalendar();
        calendar.setTimeInMillis(timestampMillis);
        return convertTime(calendar);
    }

    /**
     * Converts a time instant contained ina date object to a
     * decimal year.
     *
     * @param date a time instance to be converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final Date date) {
        if (date == null) {
            return null;
        }

        final GregorianCalendar calendar = new GregorianCalendar();
        calendar.setTime(date);
        return convertTime(calendar);
    }

    /**
     * Converts a time instant contained in a gregorian calendar to a
     * decimal year.
     *
     * @param calendar calendar containing a specific instant to be
     *                 converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final GregorianCalendar calendar) {
        if (calendar == null) {
            return null;
        }

        return WMMEarthMagneticFluxDensityEstimator.convertTime(calendar);
    }

    /**
     * Converts provided ECEF position to position expressed in NED
     * coordinates.
     *
     * @param position ECEF position to be converted.
     * @return converted position expressed in NED coordinates.
     */
    private static NEDPosition convertPosition(final ECEFPosition position) {
        final NEDVelocity velocity = new NEDVelocity();
        final NEDPosition result = new NEDPosition();
        ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                position.getX(), position.getY(), position.getZ(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }
}
File Line
com/irurueta/navigation/lateration/HomogeneousLinearLeastSquaresLateration2DSolver.java 130
com/irurueta/navigation/lateration/NonLinearLeastSquaresLateration2DSolver.java 357
        internalSetCircles(circles);
    }

    /**
     * Gets number of dimensions of provided points.
     *
     * @return always returns 2 dimensions.
     */
    @Override
    public int getNumberOfDimensions() {
        return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH;
    }

    /**
     * Minimum required number of positions and distances.
     * At least 3 positions and distances will be required to linearly solve a 2D problem.
     *
     * @return minimum required number of positions and distances.
     */
    @Override
    public int getMinRequiredPositionsAndDistances() {
        return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
    }

    /**
     * Gets estimated position.
     *
     * @return estimated position.
     */
    @Override
    public Point2D getEstimatedPosition() {
        if (mEstimatedPositionCoordinates == null) {
            return null;
        }

        final InhomogeneousPoint2D position = new InhomogeneousPoint2D();
        getEstimatedPosition(position);
        return position;
    }

    /**
     * Internally sets circles defining positions and euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 3.
     */
    private void internalSetCircles(final Circle[] circles) {
        if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final Point2D[] positions = new Point2D[circles.length];
        final double[] distances = new double[circles.length];
        for (int i = 0; i < circles.length; i++) {
            final Circle circle = circles[i];
            positions[i] = circle.getCenter();
            distances[i] = circle.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1577
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3847
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3910
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 359
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2062
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 353
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1585
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 359
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 359
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2065
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROMedSRobustEstimator<PreliminaryResult> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5559
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5907
            mB.setElementAtIndex(2, mBiasZ);

            mG.setElementAt(0, 0, g11);
            mG.setElementAt(1, 0, g21);
            mG.setElementAt(2, 0, g31);

            mG.setElementAt(0, 1, g12);
            mG.setElementAt(1, 1, g22);
            mG.setElementAt(2, 1, g32);

            mG.setElementAt(0, 2, g13);
            mG.setElementAt(1, 2, g23);
            mG.setElementAt(2, 2, g33);

            getAccelerometerBiasAsMatrix(mBa);
            getAccelerometerMa(mMa);

            // fix measured accelerometer value to obtain true
            // specific force
            mAccelerationFixer.fix(mFmeas, mFtrue);
            mG.multiply(mFtrue, mTmp);

            mInvM.multiply(mMeasAngularRate, mTrueAngularRate);
            mTrueAngularRate.subtract(mB);
            mTrueAngularRate.subtract(mTmp);

            final double norm = Utils.normF(mTrueAngularRate);
            return norm * norm;

        } catch (final AlgebraException e) {
            throw new EvaluationException(e);
        }
    }

    /**
     * Computes estimated true angular rate squared norm using current measured
     * angular rate and provided parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fittin needed for calibration computation.
     *
     * @param m11 element 1,1 of cross-coupling error matrix.
     * @param m21 element 2,1 of cross-coupling error matrix.
     * @param m31 element 3,1 of cross-coupling error matrix.
     * @param m12 element 1,2 of cross-coupling error matrix.
     * @param m22 element 2,2 of cross-coupling error matrix.
     * @param m32 element 3,2 of cross-coupling error matrix.
     * @param m13 element 1,3 of cross-coupling error matrix.
     * @param m23 element 2,3 of cross-coupling error matrix.
     * @param m33 element 3,3 of cross-coupling error matrix.
     * @return estimated true angular rate squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluate(final double m11, final double m21, final double m31,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3895
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4017
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3689
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3811
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5561
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7074
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5757
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6883
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must be
     *                                      3x1 and is expressed in radians
     *                                      per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5830
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7421
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6029
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7212
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2762
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2934
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 7253
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 7376
        }
    }

    /**
     * Converts a time instance expressed in milliseconds since epoch time
     * (January 1st, 1970 at midnight) to a decimal year.
     *
     * @param timestampMillis milliseconds value to be converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final Long timestampMillis) {
        if (timestampMillis == null) {
            return null;
        }

        final GregorianCalendar calendar = new GregorianCalendar();
        calendar.setTimeInMillis(timestampMillis);
        return convertTime(calendar);
    }

    /**
     * Converts a time instant contained ina date object to a
     * decimal year.
     *
     * @param date a time instance to be converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final Date date) {
        if (date == null) {
            return null;
        }

        final GregorianCalendar calendar = new GregorianCalendar();
        calendar.setTime(date);
        return convertTime(calendar);
    }

    /**
     * Converts a time instant contained in a gregorian calendar to a
     * decimal year.
     *
     * @param calendar calendar containing a specific instant to be
     *                 converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final GregorianCalendar calendar) {
        if (calendar == null) {
            return null;
        }

        return WMMEarthMagneticFluxDensityEstimator.convertTime(calendar);
    }

    /**
     * Converts provided ECEF position to position expressed in NED
     * coordinates.
     *
     * @param position ECEF position to be converted.
     * @return converted position expressed in NED coordinates.
     */
    private static NEDPosition convertPosition(final ECEFPosition position) {
        final NEDVelocity velocity = new NEDVelocity();
        final NEDPosition result = new NEDPosition();
        ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                position.getX(), position.getY(), position.getZ(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5213
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 12084
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 12680
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5523
    }

    /**
     * Converts provided NED position expressed in terms of latitude, longitude and height respect
     * mean Earth surface, to position expressed in ECEF coordinates.
     *
     * @param position NED position to be converted.
     * @return converted position expressed in ECEF coordinates.
     */
    private static ECEFPosition convertPosition(final NEDPosition position) {
        final ECEFVelocity velocity = new ECEFVelocity();
        final ECEFPosition result = new ECEFPosition();
        NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                position.getLatitude(), position.getLongitude(), position.getHeight(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(),
                acceleration.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second.
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value.
     */
    private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
        return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(),
                angularSpeed.getUnit(), AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Converts time instance to seconds.
     *
     * @param time time instance to be converted.
     * @return converted value.
     */
    private static double convertTime(final Time time) {
        return TimeConverter.convert(time.getValue().doubleValue(),
                time.getUnit(), TimeUnit.SECOND);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 800
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 785
                                    LMedSRobustEasyGyroscopeCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            setupAccelerationFixer();

            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            innerEstimator.setStopThreshold(mStopThreshold);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4808
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6130
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4888
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5048
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6210
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6370
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5040
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6422
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5126
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5289
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6508
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6671
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised
     *                              by this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 7027
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4867
        initialM.add(getInitialMa());

        // Force initial M to be upper diagonal
        initialM.setElementAt(1, 0, 0.0);
        initialM.setElementAt(2, 0, 0.0);
        initialM.setElementAt(2, 1, 0.0);

        mFitter.setFunctionEvaluator(
                new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
                    @Override
                    public int getNumberOfDimensions() {
                        // Input points are measured specific force coordinates
                        return BodyKinematics.COMPONENTS;
                    }

                    @Override
                    public double[] createInitialParametersArray() {
                        final double[] initial = new double[COMMON_Z_AXIS_UNKNOWNS];

                        // upper diagonal cross coupling errors M
                        int k = 0;
                        for (int j = 0; j < BodyKinematics.COMPONENTS; j++) {
                            for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(final int i, final double[] point,
                                           final double[] params, final double[] derivatives)
                            throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5182
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5349
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4976
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5143
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5295
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6788
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5466
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6617
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5546
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7111
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5729
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6928
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/indoor/fingerprint/SecondOrderNonLinearFingerprintPositionEstimator3D.java 232
com/irurueta/navigation/indoor/fingerprint/ThirdOrderNonLinearFingerprintPositionEstimator3D.java 444
        final double value1 = -10.0 * n * diffX1a / (ln10 * d1a2);
        final double value2 = -10.0 * n * diffY1a / (ln10 * d1a2);
        final double value3 = -10.0 * n * diffZ1a / (ln10 * d1a2);
        final double value4 = -5.0 * n * (-diffX1a2 + diffY1a2 + diffZ1a2) / (ln10 * d1a4);
        final double value5 = -5.0 * n * (diffX1a2 - diffY1a2 + diffZ1a2) / (ln10 * d1a4);
        final double value6 = -5.0 * n * (diffX1a2 + diffY1a2 - diffZ1a2) / (ln10 * d1a4);
        final double value7 = 20.0 * n * diffX1a * diffY1a / (ln10 * d1a4);
        final double value8 = 20.0 * n * diffY1a * diffZ1a / (ln10 * d1a4);
        final double value9 = 20.0 * n * diffX1a * diffZ1a / (ln10 * d1a4);

        final double result = pr
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1577
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1683
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 353
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1701
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1585
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3847
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 359
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3910
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 359
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 359
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2065
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2062
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4673
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5995
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4893
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6280
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 580
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2284
            setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 581
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2286
            setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 421
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1419
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1651
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1387
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3915
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 426
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3978
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 425
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2129
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<PreliminaryResult> innerEstimator =
                new PROSACRobustEstimator<>(
                        new PROSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1380
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2902
        this(measurements, commonAxisUsed, magneticModel, hardIron);
        mListener = listener;
    }

    /**
     * Gets x-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return x-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronX() {
        return mHardIronX;
    }

    /**
     * Sets x-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronX x coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final double hardIronX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mHardIronX = hardIronX;
    }

    /**
     * Gets y-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return y-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronY() {
        return mHardIronY;
    }

    /**
     * Sets y-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronY y coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final double hardIronY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mHardIronY = hardIronY;
    }

    /**
     * Gets z-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return z-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronZ() {
        return mHardIronZ;
    }

    /**
     * Sets z-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronZ z coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final double hardIronZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mHardIronZ = hardIronZ;
    }

    /**
     * Sets known hard-iron bias coordinates of magnetometer expressed
     * in Teslas (T).
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final double hardIronX,
            final double hardIronY,
            final double hardIronZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mHardIronX = hardIronX;
        mHardIronY = hardIronY;
        mHardIronZ = hardIronZ;
    }

    /**
     * Gets a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<? extends FrameBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingRadioSourceEstimator2D.java 176
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator2D.java 412
        super(readings, initialPosition, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Robustly estimates position for a radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException,
            RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<Solution<Point2D>> innerEstimator =
                new LMedSRobustEstimator<>(
                        new LMedSRobustEstimatorListener<Solution<Point2D>>() {
                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] sampleIndices,
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingRadioSourceEstimator3D.java 177
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator3D.java 412
        super(readings, initialPosition, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Robustly estimates position for a radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException,
            RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<Solution<Point3D>> innerEstimator =
                new LMedSRobustEstimator<>(
                        new LMedSRobustEstimatorListener<Solution<Point3D>>() {
                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] sampleIndices,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 7071
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3412
                        return evaluateCommonAxis(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double m11 = result[0];

        final double m12 = result[1];
        final double m22 = result[2];

        final double m13 = result[3];
        final double m23 = result[4];
        final double m33 = result[5];

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, 0.0);
        m.setElementAtIndex(2, 0.0);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        setResult(m);
    }

    /**
     * Makes proper conversion of internal cross-coupling and bias matrices.
     *
     * @param m internal cross-coupling matrix.
     */
    private void setResult(final Matrix m) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7666
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5142
        final Matrix initialB = invInitialM.multiplyAndReturnNew(initialBa);

        mFitter.setFunctionEvaluator(
                new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
                    @Override
                    public int getNumberOfDimensions() {
                        // Input points are measured specific force coordinates
                        return BodyKinematics.COMPONENTS;
                    }

                    @Override
                    public double[] createInitialParametersArray() {
                        final double[] initial = new double[COMMON_Z_AXIS_UNKNOWNS];

                        // biases b
                        for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                            initial[i] = initialB.getElementAtIndex(i);
                        }

                        // upper diagonal cross coupling errors M
                        int k = BodyKinematics.COMPONENTS;
                        for (int j = 0; j < BodyKinematics.COMPONENTS; j++) {
                            for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(final int i, final double[] point,
                                           final double[] params, final double[] derivatives)
                            throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 728
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 190
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 196
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 192
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 733
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 193
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7424
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8833
            final double[] qualityScores, final ECEFPosition position,
            final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7794
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9284
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 953
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 953
        super(position, measurements, commonAxisUsed, hardIron, initialMm,
                listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates magnetometer calibration parameters containing soft-iron
     * scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/frames/ECIorECEFFrame.java 101
com/irurueta/navigation/inertial/ECEFPosition.java 96
        setCoordinateTransformation(c);
    }

    /**
     * Gets cartesian x coordinate of body position expressed in meters (m) and resolved along ECI or ECEF-frame axes.
     *
     * @return cartesian x coordinate of body position.
     */
    public double getX() {
        return mX;
    }

    /**
     * Sets cartesian x coordinate of body position expressed in meters (m) and resolved along ECI or ECEF-frame axes.
     *
     * @param x cartesian x coordinate of body position.
     */
    public void setX(final double x) {
        mX = x;
    }

    /**
     * Gets cartesian y coordinate of body position expressed in meters (m) and resolved along ECI or ECEF-frame axes.
     *
     * @return cartesian y coordinate of body position.
     */
    public double getY() {
        return mY;
    }

    /**
     * Sets cartesian y coordinate of body position expressed in meters (m) and resolved along ECI or ECEF-frame axes.
     *
     * @param y cartesian y coordinate of body position.
     */
    public void setY(final double y) {
        mY = y;
    }

    /**
     * Gets cartesian z coordinate of body position expressed in meters (m) and resolved along ECI or ECEF-frame axes.
     *
     * @return cartesian z coordinate of body position.
     */
    public double getZ() {
        return mZ;
    }

    /**
     * Sets cartesian z coordinate of body position expressed in meters (m) and resolved along ECI or ECEF-frame axes.
     *
     * @param z cartesian z coordinate of body position.
     */
    public void setZ(final double z) {
        mZ = z;
    }

    /**
     * Sets cartesian coordinates of body position expressed in meters (m) and resolved along ECI or ECEF-frame axes.
     *
     * @param x cartesian x coordinate of body position, resolved along ECI or ECEF-frame axes.
     * @param y cartesian y coordinate of body position, resolved along ECI or ECEF-frame axes.
     * @param z cartesian z coordinate of body position, resolved along ECI or ECEF-frame axes.
     */
    public void setCoordinates(final double x, final double y, final double z) {
        mX = x;
        mY = y;
        mZ = z;
    }

    /**
     * Gets body position expressed in meters (m) and resolved along ECI or ECEF-frame axes.
     *
     * @return body position.
     */
    public Point3D getPosition() {
        return new InhomogeneousPoint3D(mX, mY, mZ);
    }

    /**
     * Gets body position expressed in meters (m) and resolved along ECI or ECEF-frame axes.
     *
     * @param result instance where position data is copied to.
     */
    public void getPosition(final Point3D result) {
        result.setInhomogeneousCoordinates(mX, mY, mZ);
    }

    /**
     * Sets body position expressed in meters (m) and resolved along ECI or ECEF-frame axes.
     *
     * @param point body position to be set.
     */
    public void setPosition(final Point3D point) {
        mX = point.getInhomX();
        mY = point.getInhomY();
        mZ = point.getInhomZ();
    }

    /**
     * Gets cartesian x coordinate of body position resolved along ECI or ECEF-frame axes.
     *
     * @param result instance where cartesian x coordinate of body position will be stored.
     */
    public void getPositionX(final Distance result) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 944
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 325
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 955
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 873
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1995
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 329
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 2026
                                    LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            innerEstimator.setStopThreshold(mStopThreshold);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 6186
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5977
            mInnerCalibrator.getEstimatedBiases(result.mEstimatedBiases);
            result.mEstimatedMg = mInnerCalibrator.getEstimatedMg();
            result.mEstimatedGg = mInnerCalibrator.getEstimatedGg();

            if (mKeepCovariance) {
                result.mCovariance = mInnerCalibrator.getEstimatedCovariance();
            } else {
                result.mCovariance = null;
            }

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mSequences.size();

            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> inlierSequences =
                    new ArrayList<>();
            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierSequences.add(mSequences.get(i));
                }
            }

            try {
                mInnerCalibrator.setGDependentCrossBiasesEstimated(mEstimateGDependentCrossBiases);
                mInnerCalibrator.setInitialBias(preliminaryResult.mEstimatedBiases);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4968
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6450
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5128
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6290
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5646
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8396
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6968
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9807
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7931
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9520
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8112
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9339
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5208
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6753
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5371
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6590
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5922
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8819
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7309
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10312
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8322
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10004
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by
     *                                      this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8514
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9810
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator3D.java 338
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 582
                                            RANSACRobustRangingRadioSourceEstimator3D.this,
                                            progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point3D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }
}
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanEpochEstimator.java 774
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanEpochEstimator.java 192
                INSLooselyCoupledKalmanState.NUM_PARAMS);

        final double gyroNoisePSD = config.getGyroNoisePSD();
        final double gyroNoiseValue = gyroNoisePSD * propagationInterval;
        for (int i = 0; i < 3; i++) {
            qPrimeMatrix.setElementAt(i, i, gyroNoiseValue);
        }

        final double accelNoisePSD = config.getAccelerometerNoisePSD();
        final double accelNoiseValue = accelNoisePSD * propagationInterval;
        for (int i = 3; i < 6; i++) {
            qPrimeMatrix.setElementAt(i, i, accelNoiseValue);
        }

        final double accelBiasPSD = config.getAccelerometerBiasPSD();
        final double accelBiasValue = accelBiasPSD * propagationInterval;
        for (int i = 9; i < 12; i++) {
            qPrimeMatrix.setElementAt(i, i, accelBiasValue);
        }

        final double gyroBiasPSD = config.getGyroBiasPSD();
        final double gyroBiasValue = gyroBiasPSD * propagationInterval;
        for (int i = 12; i < 15; i++) {
            qPrimeMatrix.setElementAt(i, i, gyroBiasValue);
        }


        // 3. Propagate state estimates using (3.14) noting that all states are zero
        // due to closed-loop correction.
        // x_est_propagated(1:15, 1) = 0;

        // 4. Propagate state estimation error covariance matrix using (3.46)
        final Matrix pMatrixOld = previousState.getCovariance();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 728
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 661
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 646
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1856
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1886
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 193
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 954
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 190
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 733
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 196
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 192
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 953
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 753
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 738
                                    MSACRobustEasyGyroscopeCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            setupAccelerationFixer();

            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7276
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8684
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7280
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7427
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8688
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8837
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7640
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9128
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7644
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7798
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9132
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9288
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised
     *                              by this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
File Line
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator2D.java 338
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator2D.java 585
                                            RANSACRobustRangingRadioSourceEstimator2D.this,
                                            progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            Solution<Point2D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5646
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9807
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6968
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8396
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5922
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10312
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7309
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8819
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1319
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 995
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double omegaMeasX = measuredKinematics.getAngularRateX();
            final double omegaMeasY = measuredKinematics.getAngularRateY();
            final double omegaMeasZ = measuredKinematics.getAngularRateZ();

            final double omegaTrueX = expectedKinematics.getAngularRateX();
            final double omegaTrueY = expectedKinematics.getAngularRateY();
            final double omegaTrueZ = expectedKinematics.getAngularRateZ();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, omegaTrueX);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1451
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 853
        final Matrix a = new Matrix(rows, GENERAL_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double omegaMeasX = measuredKinematics.getAngularRateX();
            final double omegaMeasY = measuredKinematics.getAngularRateY();
            final double omegaMeasZ = measuredKinematics.getAngularRateZ();

            final double omegaTrueX = expectedKinematics.getAngularRateX();
            final double omegaTrueY = expectedKinematics.getAngularRateY();
            final double omegaTrueZ = expectedKinematics.getAngularRateZ();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, omegaTrueX);
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 838
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 858
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 1058
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 549
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 1078
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 574
                                            PROSACRobustRangingAndRssiRadioSourceEstimator2D.this,
                                            progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point2D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 548
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 1075
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator3D.java 338
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 582
                                            PROSACRobustRangingRadioSourceEstimator3D.this,
                                            progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point3D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }


    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 728
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 809
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 190
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 819
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 661
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 733
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 646
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1856
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 196
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1886
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 192
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 193
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 953
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 954
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(
            final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(
            final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 6189
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 6448
            final Matrix estimatedMa = preliminaryResult.mEstimatedMa;

            if (mIdentity == null) {
                mIdentity = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (mTmp1 == null) {
                mTmp1 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (mTmp2 == null) {
                mTmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (mTmp3 == null) {
                mTmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            if (mTmp4 == null) {
                mTmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            mIdentity.add(estimatedMa, mTmp1);

            Utils.inverse(mTmp1, mTmp2);

            final BodyKinematics kinematics = measurement.getKinematics();
            final double fmeasX = kinematics.getFx();
            final double fmeasY = kinematics.getFy();
            final double fmeasZ = kinematics.getFz();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5841
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8597
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7158
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10011
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7585
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9164
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7759
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8993
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6124
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9035
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7516
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10531
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7966
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9630
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8143
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9453
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/lateration/PROSACRobustLateration2DSolver.java 615
com/irurueta/navigation/lateration/RANSACRobustLateration2DSolver.java 365
                                    PROSACRobustLateration2DSolver.this, progress);
                        }
                    }
                });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onSolveStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            Point2D result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (mListener != null) {
                mListener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/indoor/fingerprint/NonLinearFingerprintPositionAndRadioSourceEstimator.java 486
com/irurueta/navigation/indoor/fingerprint/NonLinearFingerprintPositionEstimator.java 326
        if (fallbackRssiStandardDeviation < TINY) {
            throw new IllegalArgumentException();
        }
        mFallbackRssiStandardDeviation = fallbackRssiStandardDeviation;
    }

    /**
     * Indicates whether measured RSSI standard deviation of closest fingerprint must be
     * propagated into measured RSSI reading variance at unknown location.
     *
     * @return true to propagate RSSI standard deviation of closest fingerprint,
     * false otherwise.
     */
    public boolean isFingerprintRssiStandardDeviationPropagated() {
        return mPropagateFingerprintRssiStandardDeviation;
    }

    /**
     * Specifies whether measured RSSI standard deviation of closest fingerprint must be
     * propagated into measured RSSI reading variance at unknown location.
     *
     * @param propagateFingerprintRssiStandardDeviation true to propagate RSSI standard
     *                                                  deviation of closest fingerprint,
     *                                                  false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setFingerprintRssiStandardDeviationPropagated(
            final boolean propagateFingerprintRssiStandardDeviation) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mPropagateFingerprintRssiStandardDeviation =
                propagateFingerprintRssiStandardDeviation;
    }

    /**
     * Indicates whether path-loss exponent standard deviation of radio source must be
     * propagated into measured RSSI reading variance at unknown location.
     *
     * @return true to propagate  path-loss exponent standard deviation of radio source,
     * false otherwise.
     */
    public boolean isPathlossExponentStandardDeviationPropagated() {
        return mPropagatePathlossExponentStandardDeviation;
    }

    /**
     * Specifies whether path-loss exponent standard deviation of radio source must be
     * propagated into measured RSSI reading variance at unknown location.
     *
     * @param propagatePathlossExponentStandardDeviation true to propagate path-loss
     *                                                   exponent standard deviation of
     *                                                   radio source, false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setPathlossExponentStandardDeviationPropagated(
            final boolean propagatePathlossExponentStandardDeviation) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mPropagatePathlossExponentStandardDeviation =
                propagatePathlossExponentStandardDeviation;
    }

    /**
     * Indicates whether covariance of closest fingerprint position must be propagated
     * into measured RSSI reading variance at unknown location.
     *
     * @return true to propagate fingerprint position covariance, false otherwise.
     */
    public boolean isFingerprintPositionCovariancePropagated() {
        return mPropagateFingerprintPositionCovariance;
    }

    /**
     * Specifies whether covariance of closest fingerprint position must be propagated
     * into measured RSSI reading variance at unknown location.
     *
     * @param propagateFingerprintPositionCovariance true to propagate fingerprint
     *                                               position covariance, false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setFingerprintPositionCovariancePropagated(
            final boolean propagateFingerprintPositionCovariance) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mPropagateFingerprintPositionCovariance =
                propagateFingerprintPositionCovariance;
    }

    /**
     * Indicates whether covariance of radio source position must be propagated into
     * measured RSSI reading variance at unknown location.
     *
     * @return true to propagate radio source position covariance, false otherwise.
     */
    public boolean isRadioSourcePositionCovariancePropagated() {
        return mPropagateRadioSourcePositionCovariance;
    }

    /**
     * Specifies whether covariance of radio source position must be propagated into
     * measured RSSI reading variance at unknown location.
     *
     * @param propagateRadioSourcePositionCovariance true to propagate radio source
     *                                               position covariance, false otherwise.
     * @throws LockedException if estimator is locked.
     */
    public void setRadioSourcePositionCovariancePropagated(
            final boolean propagateRadioSourcePositionCovariance) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mPropagateRadioSourcePositionCovariance =
                propagateRadioSourcePositionCovariance;
    }
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 1063
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator3D.java 574
                                            PROSACRobustRangingAndRssiRadioSourceEstimator3D.this,
                                            progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            Solution<Point3D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator3D.java 2840
com/irurueta/navigation/indoor/radiosource/RssiRadioSourceEstimator3D.java 433
    }

    /**
     * Gets estimated located radio source with estimated transmitted power.
     *
     * @return estimated located radio source with estimated transmitted power or null.
     */
    @Override
    @SuppressWarnings("unchecked")
    public RadioSourceWithPowerAndLocated<Point3D> getEstimatedRadioSource() {
        final List<? extends RssiReadingLocated<S, Point3D>> readings = getReadings();
        if (readings == null || readings.isEmpty()) {
            return null;
        }
        final S source = readings.get(0).getSource();

        final Point3D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        final Double transmittedPowerVariance =
                getEstimatedTransmittedPowerVariance();
        final Double transmittedPowerStandardDeviation = transmittedPowerVariance != null ?
                Math.sqrt(transmittedPowerVariance) : null;

        final Double pathlossExponentVariance =
                getEstimatedPathLossExponentVariance();
        final Double pathlossExponentStandardDeviation = pathlossExponentVariance != null ?
                Math.sqrt(pathlossExponentVariance) : null;

        if (source instanceof WifiAccessPoint) {
            final WifiAccessPoint accessPoint = (WifiAccessPoint) source;
            return new WifiAccessPointWithPowerAndLocated3D(accessPoint.getBssid(),
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 900
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 278
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 909
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 826
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1947
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 286
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1978
                                    MSACRobustKnownBiasAndPositionAccelerometerCalibrator.this, progress);
                        }
                    }
                });

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4085
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4230
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3879
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4024
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5841
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10011
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7158
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8597
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7356
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8911
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7501
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8764
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg, initialGg,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg, initialGg,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6124
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10531
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7516
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9035
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7719
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9365
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7878
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9209
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/gnss/GNSSKalmanFilteredEstimator.java 267
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1151
    public void setListener(final GNSSKalmanFilteredEstimatorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Gets minimum epoch interval expressed in seconds (s) between consecutive
     * propagations or measurements expressed in seconds.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @return minimum epoch interval between consecutive propagations or
     * measurements.
     */
    public double getEpochInterval() {
        return mEpochInterval;
    }

    /**
     * Sets minimum epoch interval expressed in seconds (s) between consecutive
     * propagations or measurements expressed in seconds.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @param epochInterval minimum epoch interval expressed in seconds (s) between
     *                      consecutive propagations or measurements.
     * @throws LockedException          if this estimator is already running.
     * @throws IllegalArgumentException if provided epoch interval is negative.
     */
    public void setEpochInterval(final double epochInterval) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (epochInterval < 0.0) {
            throw new IllegalArgumentException();
        }

        mEpochInterval = epochInterval;
    }

    /**
     * Gets minimum epoch interval between consecutive propagations or measurements.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @param result instance where minimum epoch interval will be stored.
     */
    public void getEpochIntervalAsTime(final Time result) {
        result.setValue(mEpochInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Gets minimum epoch interval between consecutive propagations or measurements.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @return minimum epoch interval.
     */
    public Time getEpochIntervalAsTime() {
        return new Time(mEpochInterval, TimeUnit.SECOND);
    }

    /**
     * Sets minimum epoch interval between consecutive propagations or measurements.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @param epochInterval minimum epoch interval.
     * @throws LockedException          if this estimator is already running.
     * @throws IllegalArgumentException if provided epoch interval is negative.
     */
    public void setEpochInterval(final Time epochInterval) throws LockedException {
        final double epochIntervalSeconds = TimeConverter.convert(
                epochInterval.getValue().doubleValue(),
                epochInterval.getUnit(), TimeUnit.SECOND);
        setEpochInterval(epochIntervalSeconds);
    }

    /**
     * Gets GNSS Kalman configuration parameters (usually obtained through
     * calibration).
     *
     * @param result instance where GNSS Kalman configuration parameters will be
     *               stored.
     * @return true if result instance is updated, false otherwise.
     */
    public boolean getConfig(final GNSSKalmanConfig result) {
File Line
com/irurueta/navigation/indoor/position/PositionEstimator.java 75
com/irurueta/navigation/indoor/position/RobustPositionEstimator.java 138
    public PositionEstimator(final L listener) {
        mListener = listener;
    }

    /**
     * Gets located radio sources ussed for lateration.
     *
     * @return located radio sources used for lateration.
     */
    public List<? extends RadioSourceLocated<P>> getSources() {
        return mSources;
    }

    /**
     * Sets located radio sources used for lateration.
     *
     * @param sources located radio sources used for lateration.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is null or the number of provided
     *                                  sources is less than the required minimum.
     */
    public void setSources(final List<? extends RadioSourceLocated<P>> sources)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        internalSetSources(sources);
    }

    /**
     * Gets fingerprint containing readings at an unknown location for provided located
     * radio sources.
     *
     * @return fingerprint containing readings at an unknown location for provided
     * located radio sources.
     */
    public Fingerprint<? extends RadioSource, ? extends R> getFingerprint() {
        return mFingerprint;
    }

    /**
     * Sets fingerprint containing readings at an unknown location for provided located
     * radio sources.
     *
     * @param fingerprint fingerprint containing readings at an unknown location for
     *                    provided located radio sources.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is null.
     */
    public void setFingerprint(final Fingerprint<? extends RadioSource, ? extends R> fingerprint)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        internalSetFingerprint(fingerprint);
    }

    /**
     * Gets listener to be notified of events raised by this instance.
     *
     * @return listener to be notified of events raised by this instance.
     */
    public L getListener() {
        return mListener;
    }

    /**
     * Sets listener to be notified of events raised by this instance.
     *
     * @param listener listener to be notified of events raised by this instance.
     * @throws LockedException if estimator is locked.
     */
    public void setListener(final L listener) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mListener = listener;
    }

    /**
     * Gets estimated inhomogeneous position coordinates.
     *
     * @return estimated inhomogeneous position coordinates.
     */
    public double[] getEstimatedPositionCoordinates() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5266
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5430
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5060
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5224
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 560
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2263
            setupWmmEstimator();

            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 556
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2264
            setupWmmEstimator();

            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/gnss/GNSSKalmanFilteredEstimator.java 267
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1151
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java 1049
    public void setListener(final GNSSKalmanFilteredEstimatorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Gets minimum epoch interval expressed in seconds (s) between consecutive
     * propagations or measurements expressed in seconds.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @return minimum epoch interval between consecutive propagations or
     * measurements.
     */
    public double getEpochInterval() {
        return mEpochInterval;
    }

    /**
     * Sets minimum epoch interval expressed in seconds (s) between consecutive
     * propagations or measurements expressed in seconds.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @param epochInterval minimum epoch interval expressed in seconds (s) between
     *                      consecutive propagations or measurements.
     * @throws LockedException          if this estimator is already running.
     * @throws IllegalArgumentException if provided epoch interval is negative.
     */
    public void setEpochInterval(final double epochInterval) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        if (epochInterval < 0.0) {
            throw new IllegalArgumentException();
        }

        mEpochInterval = epochInterval;
    }

    /**
     * Gets minimum epoch interval between consecutive propagations or measurements.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @param result instance where minimum epoch interval will be stored.
     */
    public void getEpochIntervalAsTime(final Time result) {
        result.setValue(mEpochInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Gets minimum epoch interval between consecutive propagations or measurements.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @return minimum epoch interval.
     */
    public Time getEpochIntervalAsTime() {
        return new Time(mEpochInterval, TimeUnit.SECOND);
    }

    /**
     * Sets minimum epoch interval between consecutive propagations or measurements.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @param epochInterval minimum epoch interval.
     * @throws LockedException          if this estimator is already running.
     * @throws IllegalArgumentException if provided epoch interval is negative.
     */
    public void setEpochInterval(final Time epochInterval) throws LockedException {
        final double epochIntervalSeconds = TimeConverter.convert(
                epochInterval.getValue().doubleValue(),
                epochInterval.getUnit(), TimeUnit.SECOND);
        setEpochInterval(epochIntervalSeconds);
    }

    /**
     * Gets GNSS Kalman configuration parameters (usually obtained through
     * calibration).
     *
     * @param result instance where GNSS Kalman configuration parameters will be
     *               stored.
     * @return true if result instance is updated, false otherwise.
     */
    public boolean getConfig(final GNSSKalmanConfig result) {
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 842
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 858
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 855
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
File Line
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator.java 2369
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator.java 2232
        }

        mRssiEstimator.setProgressDelta(2.0f * mProgressDelta);
        mRssiEstimator.setConfidence(mRssiConfidence);
        mRssiEstimator.setMaxIterations(mRssiMaxIterations);
        mRssiEstimator.setResultRefined(mRefineResult);
        mRssiEstimator.setCovarianceKept(mKeepCovariance);

        //initial position is not set because position estimated from ranging measures
        //will be later used
        mRssiEstimator.setInitialTransmittedPowerdBm(mInitialTransmittedPowerdBm);
        mRssiEstimator.setInitialPathLossExponent(mInitialPathLossExponent);

        mRssiEstimator.setTransmittedPowerEstimationEnabled(
                mTransmittedPowerEstimationEnabled);
        mRssiEstimator.setPathLossEstimationEnabled(mPathLossEstimationEnabled);

        mRssiEstimator.setListener(new RobustRssiRadioSourceEstimatorListener<S, P>() {
            @Override
            public void onEstimateStart(
                    final RobustRssiRadioSourceEstimator<S, P> estimator) { /* not used */ }

            @Override
            public void onEstimateEnd(
                    final RobustRssiRadioSourceEstimator<S, P> estimator) { /* not used */ }

            @Override
            public void onEstimateNextIteration(
                    final RobustRssiRadioSourceEstimator<S, P> estimator,
                    final int iteration) { /* not used */ }

            @Override
            public void onEstimateProgressChange(
                    final RobustRssiRadioSourceEstimator<S, P> estimator,
                    final float progress) {
                if (mListener != null) {
                    mListener.onEstimateProgressChange(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 929
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1070
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4535
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2017
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2159
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4312
        for (final FrameBodyMagneticFluxDensity measurement : mMeasurements) {
            final BodyMagneticFluxDensity measuredMagneticFluxDensity =
                    measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final ECEFFrame ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final double year = measurement.getYear();

            final double latitude = nedFrame.getLatitude();
            final double longitude = nedFrame.getLongitude();
            final double height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body mangetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb,
                    expectedMagneticFluxDensity);

            final double bMeasX = measuredMagneticFluxDensity.getBx();
            final double bMeasY = measuredMagneticFluxDensity.getBy();
            final double bMeasZ = measuredMagneticFluxDensity.getBz();

            final double bTrueX = expectedMagneticFluxDensity.getBx();
            final double bTrueY = expectedMagneticFluxDensity.getBy();
            final double bTrueZ = expectedMagneticFluxDensity.getBz();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1164
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2248
    }

    /**
     * Fills scale factor and cross coupling error matrix with estimated values.
     *
     * @param sx  x scale factor
     * @param sy  y scale factor
     * @param sz  z scale factor
     * @param mxy x-y cross coupling
     * @param mxz x-z cross coupling
     * @param myx y-x cross coupling
     * @param myz y-z cross coupling
     * @param mzx z-x cross coupling
     * @param mzy z-y cross coupling
     * @throws WrongSizeException never happens.
     */
    private void fillMm(final double sx, final double sy, final double sz,
                        final double mxy, final double mxz, final double myx,
                        final double myz, final double mzx, final double mzy)
            throws WrongSizeException {
        if (mEstimatedMm == null) {
            mEstimatedMm = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                    BodyMagneticFluxDensity.COMPONENTS);
        }

        mEstimatedMm.setElementAt(0, 0, sx);
        mEstimatedMm.setElementAt(1, 0, myx);
        mEstimatedMm.setElementAt(2, 0, mzx);

        mEstimatedMm.setElementAt(0, 1, mxy);
        mEstimatedMm.setElementAt(1, 1, sy);
        mEstimatedMm.setElementAt(2, 1, mzy);

        mEstimatedMm.setElementAt(0, 2, mxz);
        mEstimatedMm.setElementAt(1, 2, myz);
        mEstimatedMm.setElementAt(2, 2, sz);
    }
}
File Line
com/irurueta/navigation/indoor/RadioSourceKNearestFinder.java 159
com/irurueta/navigation/indoor/RadioSourceNoMeanKNearestFinder.java 163
        final List<Double> nearestSqrDistances = new ArrayList<>();
        findKNearestTo(fingerprint, fingerprints, k, result, nearestSqrDistances);

        return result;
    }

    /**
     * Finds k-nearest fingerprints to provided one, in terms of signal euclidean distances, within the collection
     * of provided fingerprints.
     *
     * @param fingerprint         fingerprint to find the k-nearest ones to.
     * @param fingerprints        collection of fingerprints ot make the search for the nearest ones.
     * @param k                   number of nearest fingerprints to find.
     * @param nearestFingerprints list where found nearest fingerprints will be stored ordered from closest to farthest
     *                            or an empty list if none could be found.
     * @param nearestSqrDistances list where squared signal euclidean distances corresponding to found fingerprints will
     *                            be stored or an empty list if no fingerprint is found.
     * @param <P>                 a {@link Point} type.
     * @param <S>                 a {@link RadioSource} type.
     * @throws IllegalArgumentException if any parameter is null or k is less than 1.
     */
    @SuppressWarnings("Duplicates")
    public static <P extends Point<?>, S extends RadioSource> void findKNearestTo(
            final RssiFingerprint<S, RssiReading<S>> fingerprint,
            final Collection<? extends RssiFingerprintLocated<S, RssiReading<S>, P>> fingerprints,
            final int k,
            final List<RssiFingerprintLocated<S, RssiReading<S>, P>> nearestFingerprints,
            final List<Double> nearestSqrDistances) {

        if (fingerprint == null || fingerprints == null || k < 1 || nearestFingerprints == null ||
                nearestSqrDistances == null) {
            throw new IllegalArgumentException();
        }

        nearestSqrDistances.clear();
        nearestFingerprints.clear();

        double maxSqrDist = Double.MAX_VALUE;
        for (RssiFingerprintLocated<S, RssiReading<S>, P> f : fingerprints) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1476
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 1006
    }

    /**
     * Fills scale factor and cross coupling error matrix with estimated values.
     *
     * @param sx  x scale factor
     * @param sy  y scale factor
     * @param sz  z scale factor
     * @param mxy x-y cross coupling
     * @param mxz x-z cross coupling
     * @param myx y-x cross coupling
     * @param myz y-z cross coupling
     * @param mzx z-x cross coupling
     * @param mzy z-y cross coupling
     * @throws WrongSizeException never happens.
     */
    private void fillMa(final double sx, final double sy, final double sz,
                        final double mxy, final double mxz, final double myx,
                        final double myz, final double mzx, final double mzy)
            throws WrongSizeException {
        if (mEstimatedMa == null) {
            mEstimatedMa = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
        }

        mEstimatedMa.setElementAt(0, 0, sx);
        mEstimatedMa.setElementAt(1, 0, myx);
        mEstimatedMa.setElementAt(2, 0, mzx);

        mEstimatedMa.setElementAt(0, 1, mxy);
        mEstimatedMa.setElementAt(1, 1, sy);
        mEstimatedMa.setElementAt(2, 1, mzy);

        mEstimatedMa.setElementAt(0, 2, mxz);
        mEstimatedMa.setElementAt(1, 2, myz);
        mEstimatedMa.setElementAt(2, 2, sz);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2667
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3046
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException |
                com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4077
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5699
            final int i, final double[] params)
            throws EvaluationException {

        final double bx = params[0];
        final double by = params[1];
        final double bz = params[2];

        final double m11 = params[3];

        final double m12 = params[4];
        final double m22 = params[5];

        final double m13 = params[6];
        final double m23 = params[7];
        final double m33 = params[8];

        final double g11 = params[9];
        final double g21 = params[10];
        final double g31 = params[11];

        final double g12 = params[12];
        final double g22 = params[13];
        final double g32 = params[14];

        final double g13 = params[15];
        final double g23 = params[16];
        final double g33 = params[17];

        return evaluate(i, bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3748
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5329
            final int i, final double[] params)
            throws EvaluationException {

        final double m11 = params[0];
        final double m21 = params[1];
        final double m31 = params[2];

        final double m12 = params[3];
        final double m22 = params[4];
        final double m32 = params[5];

        final double m13 = params[6];
        final double m23 = params[7];
        final double m33 = params[8];

        final double g11 = params[9];
        final double g21 = params[10];
        final double g31 = params[11];

        final double g12 = params[12];
        final double g22 = params[13];
        final double g32 = params[14];

        final double g13 = params[15];
        final double g23 = params[16];
        final double g33 = params[17];

        return evaluate(i, m11, m21, m31, m12, m22, m32,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5978
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 11938
            final Matrix m1 = Matrix.identity(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            m1.add(mg);

            final Matrix angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            angularRateTrue.setElementAtIndex(0, angularRateTrueX);
            angularRateTrue.setElementAtIndex(1, angularRateTrueY);
            angularRateTrue.setElementAtIndex(2, angularRateTrueZ);

            m1.multiply(angularRateTrue);

            final Matrix fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            fTrue.setElementAtIndex(0, fTrueX);
            fTrue.setElementAtIndex(1, fTrueY);
            fTrue.setElementAtIndex(2, fTrueZ);
            final Matrix m2 = gg.multiplyAndReturnNew(fTrue);

            m1.add(m2);

            final double angularRateMeasX2 = mBiasX + m1.getElementAtIndex(0);
            final double angularRateMeasY2 = mBiasY + m1.getElementAtIndex(1);
            final double angularRateMeasZ2 = mBiasZ + m1.getElementAtIndex(2);

            final double diffX = angularRateMeasX2 - angularRateMeasX1;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4740
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6062
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 2780
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 12528
            final Matrix m1 = Matrix.identity(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            m1.add(mg);

            final Matrix angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            angularRateTrue.setElementAtIndex(0, angularRateTrueX);
            angularRateTrue.setElementAtIndex(1, angularRateTrueY);
            angularRateTrue.setElementAtIndex(2, angularRateTrueZ);

            m1.multiply(angularRateTrue);

            final Matrix fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            fTrue.setElementAtIndex(0, fTrueX);
            fTrue.setElementAtIndex(1, fTrueY);
            fTrue.setElementAtIndex(2, fTrueZ);
            final Matrix m2 = gg.multiplyAndReturnNew(fTrue);

            m1.add(m2);

            final double angularRateMeasX2 = bx + m1.getElementAtIndex(0);
            final double angularRateMeasY2 = by + m1.getElementAtIndex(1);
            final double angularRateMeasZ2 = bz + m1.getElementAtIndex(2);

            final double diffX = angularRateMeasX2 - angularRateMeasX1;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4966
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6353
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6389
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6829
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1794
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1836
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3988
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3946
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4005
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4051
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return mPosition;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final NEDPosition result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (mPosition != null) {
            final NEDVelocity velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    mPosition.getX(), mPosition.getY(), mPosition.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mPosition = convertPosition(position);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2934
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3046
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3193
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException |
                com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4158
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4308
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4695
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4829
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores     quality scores corresponding to each provided
     *                          sequence. The larger the score value the better
     *                          the quality of the sequence.
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must be 3x1 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must be 3x1
     *                          and is expressed in meters per squared
     *                          second (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3952
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4102
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4489
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4623
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores     quality scores corresponding to each provided
     *                          sequence. The larger the score value the better
     *                          the quality of the sequence.
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must be 3x1 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must be 3x1
     *                          and is expressed in meters per squared
     *                          second (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5211
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6702
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5380
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6533
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5552
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8296
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6874
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9704
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5455
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7019
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by
     *                                      this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5637
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6837
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5821
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8710
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7203
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10200
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/indoor/position/PROMedSRobustRssiPositionEstimator3D.java 365
com/irurueta/navigation/indoor/position/PROSACRobustRssiPositionEstimator3D.java 363
    public PROMedSRobustRssiPositionEstimator3D(
            final double[] sourceQualityScores,
            final double[] fingerprintReadingQualityScores,
            final List<? extends RadioSourceLocated<Point3D>> sources,
            final RssiFingerprint<? extends RadioSource, ? extends RssiReading<? extends RadioSource>> fingerprint,
            final RobustRssiPositionEstimatorListener<Point3D> listener) {
        this(sources, fingerprint, listener);
        internalSetSourceQualityScores(sourceQualityScores);
        internalSetFingerprintReadingsQualityScores(fingerprintReadingQualityScores);
    }

    /**
     * Returns quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each radio source.
     */
    public double[] getSourceQualityScores() {
        return mSourceQualityScores;
    }

    /**
     * Sets quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the radio source.
     *
     * @param sourceQualityScores quality scores corresponding to each radio source.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setSourceQualityScores(final double[] sourceQualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSourceQualityScores(sourceQualityScores);
    }

    /**
     * Gets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each reading within provided
     * fingerprint.
     */
    public double[] getFingerprintReadingsQualityScores() {
        return mFingerprintReadingsQualityScores;
    }

    /**
     * Sets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @param fingerprintReadingsQualityScores quality scores corresponding to each
     *                                         reading within provided fingerprint.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 93
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 124
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD =
            RobustEstimatorMethod.LMedS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen subsamples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Known x-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasX;

    /**
     * Known y-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasY;

    /**
     * Known z-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasZ;

    /**
     * Known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSx;

    /**
     * Known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSy;

    /**
     * Known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSz;

    /**
     * Known accelerometer x-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMxy;

    /**
     * Know accelerometer x-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMxz;

    /**
     * Known accelerometer y-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMyx;

    /**
     * Known accelerometer y-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMyz;

    /**
     * Known accelerometer z-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMzx;

    /**
     * Known accelerometer z-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMzy;

    /**
     * Initial x-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     */
    private double mInitialBiasX;

    /**
     * Initial y-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     */
    private double mInitialBiasY;

    /**
     * Initial z-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     */
    private double mInitialBiasZ;

    /**
     * Initial gyroscope x scaling factor.
     */
    private double mInitialSx;

    /**
     * Initial gyroscope y scaling factor.
     */
    private double mInitialSy;

    /**
     * Initial gyroscope z scaling factor.
     */
    private double mInitialSz;

    /**
     * Initial gyroscope x-y cross coupling error.
     */
    private double mInitialMxy;

    /**
     * Initial gyroscope x-z cross coupling error.
     */
    private double mInitialMxz;

    /**
     * Initial gyroscope y-x cross coupling error.
     */
    private double mInitialMyx;

    /**
     * Initial gyroscope y-z cross coupling error.
     */
    private double mInitialMyz;

    /**
     * Initial gyroscope z-x cross coupling error.
     */
    private double mInitialMzx;

    /**
     * Initial gyroscope z-y cross coupling error.
     */
    private double mInitialMzy;

    /**
     * Initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     */
    private Matrix mInitialGg;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 93
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 124
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD =
            RobustEstimatorMethod.LMedS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen subsamples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Known x-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasX;

    /**
     * Known y-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasY;

    /**
     * Known z-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasZ;

    /**
     * Known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSx;

    /**
     * Known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSy;

    /**
     * Known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSz;

    /**
     * Known accelerometer x-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMxy;

    /**
     * Know accelerometer x-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMxz;

    /**
     * Known accelerometer y-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMyx;

    /**
     * Known accelerometer y-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMyz;

    /**
     * Known accelerometer z-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMzx;

    /**
     * Known accelerometer z-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMzy;

    /**
     * X-coordinate of gyroscope known bias expressed in radians per second
     * (rad/s).
     */
    private double mBiasX;

    /**
     * Y-coordinate of gyroscope known bias expressed in radians per second
     * (rad/s).
     */
    private double mBiasY;

    /**
     * Z-coordinate of gyroscope known bias expressed in radians per second
     * (rad/s).
     */
    private double mBiasZ;

    /**
     * Initial gyroscope x scaling factor.
     */
    private double mInitialSx;

    /**
     * Initial gyroscope y scaling factor.
     */
    private double mInitialSy;

    /**
     * Initial gyroscope z scaling factor.
     */
    private double mInitialSz;

    /**
     * Initial gyroscope x-y cross coupling error.
     */
    private double mInitialMxy;

    /**
     * Initial gyroscope x-z cross coupling error.
     */
    private double mInitialMxz;

    /**
     * Initial gyroscope y-x cross coupling error.
     */
    private double mInitialMyx;

    /**
     * Initial gyroscope y-z cross coupling error.
     */
    private double mInitialMyz;

    /**
     * Initial gyroscope z-x cross coupling error.
     */
    private double mInitialMzx;

    /**
     * Initial gyroscope z-y cross coupling error.
     */
    private double mInitialMzy;

    /**
     * Initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     */
    private Matrix mInitialGg;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4605
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5927
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4609
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4744
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5931
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6066
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4824
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6211
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4828
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4970
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6215
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6357
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised
     *                              by this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
File Line
com/irurueta/navigation/indoor/fingerprint/NonLinearFingerprintPositionAndRadioSourceEstimator.java 602
com/irurueta/navigation/indoor/fingerprint/NonLinearFingerprintPositionEstimator.java 460
    }

    /**
     * Estimates position and radio sources based on provided located radio sources and readings of
     * such radio sources at an unknown location.
     *
     * @throws LockedException                if estimator is locked.
     * @throws NotReadyException              if estimator is not ready.
     * @throws FingerprintEstimationException if estimation fails for some other reason.
     */
    @Override
    @SuppressWarnings("Duplicates")
    public void estimate() throws LockedException, NotReadyException,
            FingerprintEstimationException {

        if (!isReady()) {
            throw new NotReadyException();
        }
        if (isLocked()) {
            throw new LockedException();
        }

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            RadioSourceNoMeanKNearestFinder<P, RadioSource> noMeanfinder = null;
            RadioSourceKNearestFinder<P, RadioSource> finder = null;
            if (mUseNoMeanNearestFingerprintFinder) {
                //noinspection unchecked
                noMeanfinder = new RadioSourceNoMeanKNearestFinder<>(
                        (Collection<? extends RssiFingerprintLocated<RadioSource,
                                RssiReading<RadioSource>, P>>) mLocatedFingerprints);
            } else {
                //noinspection unchecked
                finder = new RadioSourceKNearestFinder<>(
                        (Collection<? extends RssiFingerprintLocated<RadioSource,
                                RssiReading<RadioSource>, P>>) mLocatedFingerprints);
            }

            mEstimatedPositionCoordinates = null;
            mCovariance = null;
File Line
com/irurueta/navigation/indoor/fingerprint/SecondOrderNonLinearFingerprintPositionEstimator2D.java 162
com/irurueta/navigation/indoor/fingerprint/ThirdOrderNonLinearFingerprintPositionEstimator2D.java 162
    }

    /**
     * Evaluates a non-linear multi dimension function at provided point using
     * provided parameters and returns its evaluation and derivatives of the
     * function respect the function parameters.
     *
     * @param i           number of sample being evaluated.
     * @param point       point where function will be evaluated.
     * @param params      initial parameters estimation to be tried. These will
     *                    change as the Levenberg-Marquard algorithm iterates to the best solution.
     *                    These are used as input parameters along with point to evaluate function.
     * @param derivatives partial derivatives of the function respect to each
     *                    provided parameter.
     * @return function evaluation at provided point.
     */
    @Override
    @SuppressWarnings("Duplicates")
    protected double evaluate(
            final int i, final double[] point, final double[] params,
            final double[] derivatives) {
        //This method implements received power at point pi = (xi, yi) and its derivatives

        //Pr(pi) = Pr(p1)
        //  - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
        //  - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
        //  - 5*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)^2
        //  - 5*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*d1a^4)*(yi - y1)^2
        //  + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4))*(xi - x1)*(yi - y1)

        final double xi = params[0];
        final double yi = params[1];

        //received power
        final double pr = point[0];

        //fingerprint coordinates
        final double x1 = point[1];
        final double y1 = point[2];

        //radio source coordinates
        final double xa = point[3];
        final double ya = point[4];

        //path loss exponent
        final double n = point[5];

        final double ln10 = Math.log(10.0);

        final double diffXi1 = xi - x1;
        final double diffYi1 = yi - y1;

        final double diffX1a = x1 - xa;
        final double diffY1a = y1 - ya;

        final double diffXi12 = diffXi1 * diffXi1;
        final double diffYi12 = diffYi1 * diffYi1;

        final double diffX1a2 = diffX1a * diffX1a;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4453
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4563
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      sequence. The larger the score value the better
     *                      the quality of the sequence.
     * @param sequences     collection of sequences containing timestamped body
     *                      kinematics measurements.
     * @param initialBias   initial gyroscope bias to be used to find a
     *                      solution. This must have length 3 and is expressed
     *                      in radians per second (rad/s).
     * @param initialMg     initial gyroscope scale factors and cross coupling
     *                      errors matrix. Must be 3x3.
     * @param initialGg     initial gyroscope G-dependent cross biases
     *                      introduced on the gyroscope by the specific forces
     *                      sensed by the accelerometer. Must be 3x3.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4894
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5033
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5930
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 2726
        return create(measurements, bias, commonAxisUsed, listener,
                DEFAULT_ROBUST_METHOD);
    }

    /**
     * Computes error of a preliminary result respect a given measurement.
     *
     * @param measurement       a measurement.
     * @param preliminaryResult a preliminary result.
     * @return computed error.
     */
    protected double computeError(
            final StandardDeviationFrameBodyKinematics measurement,
            final PreliminaryResult preliminaryResult) {
        // We know that measured angular rate is:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Hence:
        // [Ωmeasx] = [bx] + ( [1   0   0] + [sx    mxy    mxz]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0   1   0]   [myx   sy     myz]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0   0   1]   [mzx   mzy    sz ]  [Ωtruez]   [g31   g32   g33][ftruez]

        final BodyKinematics measuredKinematics = measurement.getKinematics();
        final ECEFFrame ecefFrame = measurement.getFrame();
        final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
        final double timeInterval = measurement.getTimeInterval();

        final BodyKinematics expectedKinematics = ECEFKinematicsEstimator
                .estimateKinematicsAndReturnNew(timeInterval, ecefFrame,
                        previousEcefFrame);

        final double angularRateMeasX1 = measuredKinematics.getAngularRateX();
        final double angularRateMeasY1 = measuredKinematics.getAngularRateY();
        final double angularRateMeasZ1 = measuredKinematics.getAngularRateZ();

        final double angularRateTrueX = expectedKinematics.getAngularRateX();
        final double angularRateTrueY = expectedKinematics.getAngularRateY();
        final double angularRateTrueZ = expectedKinematics.getAngularRateZ();

        final double fTrueX = expectedKinematics.getFx();
        final double fTrueY = expectedKinematics.getFy();
        final double fTrueZ = expectedKinematics.getFz();

        final Matrix mg = preliminaryResult.mEstimatedMg;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4247
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4357
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias,
                        initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias,
                        initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      sequence. The larger the score value the better
     *                      the quality of the sequence.
     * @param sequences     collection of sequences containing timestamped body
     *                      kinematics measurements.
     * @param initialBias   initial gyroscope bias to be used to find a
     *                      solution. This must have length 3 and is expressed
     *                      in radians per second (rad/s).
     * @param initialMg     initial gyroscope scale factors and cross coupling
     *                      errors matrix. Must be 3x3.
     * @param initialGg     initial gyroscope G-dependent cross biases
     *                      introduced on the gyroscope by the specific forces
     *                      sensed by the accelerometer. Must be 3x3.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4688
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4827
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5552
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9704
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6874
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8296
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5821
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10200
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7203
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8710
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/indoor/position/RobustMixedPositionEstimator3D.java 998
com/irurueta/navigation/indoor/position/RobustRangingAndRssiPositionEstimator2D.java 990
com/irurueta/navigation/indoor/position/RobustRangingAndRssiPositionEstimator3D.java 990
com/irurueta/navigation/indoor/position/RobustRangingPositionEstimator2D.java 998
com/irurueta/navigation/indoor/position/RobustRangingPositionEstimator3D.java 999
com/irurueta/navigation/indoor/position/RobustRssiPositionEstimator2D.java 990
com/irurueta/navigation/indoor/position/RobustRssiPositionEstimator3D.java 990
        Point3D[] positionsArray = new InhomogeneousPoint3D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];

        double[] qualityScoresArray = null;
        if (distanceQualityScores != null) {
            qualityScoresArray = new double[size];
        }

        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);

            if (qualityScoresArray != null) {
                qualityScoresArray[i] = distanceQualityScores.get(i);
            }
        }

        try {
            mLaterationSolver.setPositionsDistancesAndStandardDeviations(
                    positionsArray, distancesArray, distanceStandardDeviationsArray);

            if (qualityScoresArray != null) {
                mLaterationSolver.setQualityScores(qualityScoresArray);
            }
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator.java 1091
com/irurueta/navigation/indoor/radiosource/RangingAndRssiRadioSourceEstimator.java 1014
    public Double getEstimatedTransmittedPowerdBm() {
        return mEstimatedTransmittedPowerdBm;
    }

    /**
     * Gets estimated exponent typically used on free space for path loss propagation in
     * terms of distance.
     * On different environments path loss exponent might have different values:
     * - Free space: 2.0
     * - Urban Area: 2.7 to 3.5
     * - Suburban Area: 3 to 5
     * - Indoor (line-of-sight): 1.6 to 1.8
     * If path loss exponent estimation is not enabled, this value will always be equal to
     * {@link #DEFAULT_PATH_LOSS_EXPONENT}
     *
     * @return estimated path loss exponent.
     */
    public double getEstimatedPathLossExponent() {
        return mEstimatedPathLossExponent;
    }

    /**
     * Gets estimated transmitted power variance.
     * This value will only be available when transmitted power
     * estimation is enabled.
     *
     * @return estimated transmitted power variance or null.
     */
    public Double getEstimatedTransmittedPowerVariance() {
        return mEstimatedTransmittedPowerVariance;
    }

    /**
     * Gets estimated path loss exponent variance.
     * This value will only be available when pathloss
     * exponent estimation is enabled.
     *
     * @return estimated path loss exponent variance or null.
     */
    public Double getEstimatedPathLossExponentVariance() {
        return mEstimatedPathLossExponentVariance;
    }

    /**
     * Creates inner estimators if needed.
     */
    protected abstract void createInnerEstimatorsIfNeeded();

    /**
     * Creates a ranging reading from a ranging and RSSI reading.
     *
     * @param reading input reading to convert from.
     * @return a ranging reading containing only the ranging data of input reading.
     */
    private RangingReadingLocated<S, P> createRangingReading(
            final RangingAndRssiReadingLocated<S, P> reading) {
        return new RangingReadingLocated<>(reading.getSource(),
                reading.getDistance(), reading.getPosition(),
                reading.getDistanceStandardDeviation(),
                reading.getPositionCovariance());
    }

    /**
     * Creates an RSSI reading from a ranging and RSSI reading.
     *
     * @param reading input reading to convert from.
     * @return an RSSI reading containing only the RSSI data of input reading.
     */
    private RssiReadingLocated<S, P> createRssiReading(
            final RangingAndRssiReadingLocated<S, P> reading) {
        return new RssiReadingLocated<>(reading.getSource(),
                reading.getRssi(), reading.getPosition(),
                reading.getRssiStandardDeviation(),
                reading.getPositionCovariance());
    }
File Line
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator.java 2283
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator.java 2167
                    rangingReadings.size() < mRangingEstimator.getMinReadings();
        }

        mRangingEstimator.setProgressDelta(2.0f * mProgressDelta);
        mRangingEstimator.setConfidence(mRangingConfidence);
        mRangingEstimator.setMaxIterations(mRangingMaxIterations);
        mRangingEstimator.setResultRefined(mRefineResult);
        mRangingEstimator.setCovarianceKept(mKeepCovariance);
        mRangingEstimator.setUseReadingPositionCovariances(
                mUseReadingPositionCovariances);
        mRangingEstimator.setHomogeneousLinearSolverUsed(
                mUseHomogeneousRangingLinearSolver);

        mRangingEstimator.setInitialPosition(mInitialPosition);

        mRangingEstimator.setListener(new RobustRangingRadioSourceEstimatorListener<S, P>() {
            @Override
            public void onEstimateStart(
                    final RobustRangingRadioSourceEstimator<S, P> estimator) {
                //not used
            }

            @Override
            public void onEstimateEnd(
                    final RobustRangingRadioSourceEstimator<S, P> estimator) {
                //not used
            }

            @Override
            public void onEstimateNextIteration(
                    final RobustRangingRadioSourceEstimator<S, P> estimator,
                    final int iteration) {
                //not used
            }

            @Override
            public void onEstimateProgressChange(
                    final RobustRangingRadioSourceEstimator<S, P> estimator,
                    final float progress) {
                if (mListener != null) {
                    mListener.onEstimateProgressChange(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4888
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6370
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5048
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6210
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5748
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8496
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7065
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9910
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 12014
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 12607
            mInnerCalibrator.calibrate();

            result.mEstimatedMg = mInnerCalibrator.getEstimatedMg();
            result.mEstimatedGg = mInnerCalibrator.getEstimatedGg();

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mMeasurements.size();

            final List<StandardDeviationBodyKinematics> inlierMeasurements =
                    new ArrayList<>();
            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(mMeasurements.get(i));
                }
            }

            try {
                mInnerCalibrator.setTurntableRotationRate(mTurntableRotationRate);
                mInnerCalibrator.setTimeInterval(mTimeInterval);
                mInnerCalibrator.setGDependentCrossBiasesEstimated(mEstimateGDependentCrossBiases);
                mInnerCalibrator.setInitialMg(preliminaryResult.mEstimatedMg);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5126
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6671
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5289
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6508
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6020
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8925
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7412
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10421
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/estimators/NEDPositionEstimator.java 2281
com/irurueta/navigation/inertial/estimators/NEDVelocityEstimator.java 2383
        estimatePosition(timeInterval, oldPosition, oldVelocity, velocity,
                result);
        return result;
    }

    /**
     * Converts provided time instance to seconds (s).
     *
     * @param time time instance to be converted.
     * @return provided time value expressed in seconds.
     */
    private static double convertTimeToDouble(final Time time) {
        return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(),
                TimeUnit.SECOND);
    }

    /**
     * Converts provided angle instance to radians (rad).
     *
     * @param angle angle instance to be converted.
     * @return provided angle value expressed in radians.
     */
    private static double convertAngleToDouble(final Angle angle) {
        return AngleConverter.convert(angle.getValue().doubleValue(), angle.getUnit(), AngleUnit.RADIANS);
    }

    /**
     * Converts provided distance instance to meters (m).
     *
     * @param distance distance instance to be converted.
     * @return provided distance value expressed in meters.
     */
    private static double convertDistanceToDouble(final Distance distance) {
        return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(),
                DistanceUnit.METER);
    }

    /**
     * Converts provided speed instance to meters per second (m/s).
     *
     * @param speed speed instance to be converted.
     * @return provided speed value expressed in meters per second.
     */
    private static double convertSpeedToDouble(final Speed speed) {
        return SpeedConverter.convert(speed.getValue().doubleValue(), speed.getUnit(),
                SpeedUnit.METERS_PER_SECOND);
    }
}
File Line
com/irurueta/navigation/indoor/fingerprint/LinearFingerprintPositionEstimator.java 106
com/irurueta/navigation/indoor/fingerprint/NonLinearFingerprintPositionAndRadioSourceEstimator.java 602
com/irurueta/navigation/indoor/fingerprint/NonLinearFingerprintPositionEstimator.java 460
    }

    /**
     * Estimates position based on provided located radio sources and readings of such radio sources at
     * an unknown location.
     *
     * @throws LockedException                if estimator is locked.
     * @throws NotReadyException              if estimator is not ready.
     * @throws FingerprintEstimationException if estimation fails for some other reason.
     */
    @Override
    @SuppressWarnings("Duplicates")
    public void estimate() throws LockedException, NotReadyException,
            FingerprintEstimationException {

        if (!isReady()) {
            throw new NotReadyException();
        }
        if (isLocked()) {
            throw new LockedException();
        }

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            RadioSourceNoMeanKNearestFinder<P, RadioSource> noMeanfinder = null;
            RadioSourceKNearestFinder<P, RadioSource> finder = null;
            if (mUseNoMeanNearestFingerprintFinder) {
                //noinspection unchecked
                noMeanfinder = new RadioSourceNoMeanKNearestFinder<>(
                        (Collection<? extends RssiFingerprintLocated<RadioSource,
                                RssiReading<RadioSource>, P>>) mLocatedFingerprints);
            } else {
                //noinspection unchecked
                finder = new RadioSourceKNearestFinder<>(
                        (Collection<? extends RssiFingerprintLocated<RadioSource,
                                RssiReading<RadioSource>, P>>) mLocatedFingerprints);
            }

            mEstimatedPositionCoordinates = null;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7508
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5306
        final Matrix initialB = invInitialM.multiplyAndReturnNew(initialBa);

        mFitter.setFunctionEvaluator(
                new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
                    @Override
                    public int getNumberOfDimensions() {
                        // Input points are measured specific force coordinates
                        return BodyKinematics.COMPONENTS;
                    }

                    @Override
                    public double[] createInitialParametersArray() {
                        final double[] initial = new double[GENERAL_UNKNOWNS];

                        // biases b
                        for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                            initial[i] = initialB.getElementAtIndex(i);
                        }

                        // cross coupling errors M
                        final int num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                        for (int i = 0, j = BodyKinematics.COMPONENTS; i < num; i++, j++) {
                            initial[j] = initialM.getElementAtIndex(i);
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(final int i, final double[] point,
                                           final double[] params, final double[] derivatives)
                            throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3558
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3884
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3928
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4296
        mEstimatedMg.setElementAt(1, 1, sy);

        mEstimatedMg.setElementAt(0, 2, mxz);
        mEstimatedMg.setElementAt(1, 2, myz);
        mEstimatedMg.setElementAt(2, 2, sz);

        if (mEstimatedGg == null) {
            mEstimatedGg = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
        } else {
            mEstimatedGg.initialize(0.0);
        }

        mEstimatedGg.setElementAtIndex(0, g11);
        mEstimatedGg.setElementAtIndex(1, g21);
        mEstimatedGg.setElementAtIndex(2, g31);
        mEstimatedGg.setElementAtIndex(3, g12);
        mEstimatedGg.setElementAtIndex(4, g22);
        mEstimatedGg.setElementAtIndex(5, g32);
        mEstimatedGg.setElementAtIndex(6, g13);
        mEstimatedGg.setElementAtIndex(7, g23);
        mEstimatedGg.setElementAtIndex(8, g33);

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 95
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 95
public abstract class RobustKnownBiasTurntableGyroscopeCalibrator {
    /**
     * Indicates whether by default a common z-axis is assumed for both the accelerometer
     * and gyroscope.
     */
    public static final boolean DEFAULT_USE_COMMON_Z_AXIS = true;

    /**
     * Indicates that by default G-dependent cross biases introduced
     * by the accelerometer on the gyroscope are estimated.
     */
    public static final boolean DEFAULT_ESTIMATE_G_DEPENDENT_CROSS_BIASES = true;

    /**
     * Default turntable rotation rate.
     */
    public static final double DEFAULT_TURNTABLE_ROTATION_RATE =
            TurntableGyroscopeCalibrator.DEFAULT_TURNTABLE_ROTATION_RATE;

    /**
     * Default time interval between measurements expressed in seconds (s).
     * This is a typical value when we have 50 samples per second.
     */
    public static final double DEFAULT_TIME_INTERVAL =
            TurntableGyroscopeCalibrator.DEFAULT_TIME_INTERVAL;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD =
            RobustEstimatorMethod.LMedS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen subsamples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Known x-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasX;

    /**
     * Known y-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasY;

    /**
     * Known z-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasZ;

    /**
     * Known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSx;

    /**
     * Known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSy;

    /**
     * Known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSz;

    /**
     * Known accelerometer x-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMxy;

    /**
     * Know accelerometer x-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMxz;

    /**
     * Known accelerometer y-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMyx;

    /**
     * Known accelerometer y-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMyz;

    /**
     * Known accelerometer z-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMzx;

    /**
     * Known accelerometer z-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMzy;

    /**
     * Known x-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     */
    private double mBiasX;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 6315
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 6583
            mEstimatedMa = preliminaryResult.mEstimatedMa;
        }
    }

    /**
     * Computes gravity norm for current position.
     *
     * @return gravity norm for current position expressed in meters per squared second.
     */
    protected double computeGravityNorm() {
        final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
                mPosition.getX(), mPosition.getY(), mPosition.getZ());
        return gravity.getNorm();
    }

    /**
     * Converts provided NED position expressed in terms of latitude, longitude and height respect
     * mean Earth surface, to position expressed in ECEF coordinates.
     *
     * @param position NED position to be converted.
     * @return converted position expressed in ECEF coordinates.
     */
    private static ECEFPosition convertPosition(final NEDPosition position) {
        final ECEFVelocity velocity = new ECEFVelocity();
        final ECEFPosition result = new ECEFPosition();
        NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                position.getLatitude(), position.getLongitude(), position.getHeight(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(),
                acceleration.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Internal class containing estimated preliminary result.
     */
    protected static class PreliminaryResult {
        /**
         * Estimated accelerometer scale factors and cross coupling errors.
         * This is the product of matrix Ta containing cross coupling errors and Ka
         * containing scaling factors.
         * So tat:
         * <pre>
         *     Ma = [sx    mxy  mxz] = Ta*Ka
         *          [myx   sy   myz]
         *          [mzx   mzy  sz ]
         * </pre>
         * Where:
         * <pre>
         *     Ka = [sx 0   0 ]
         *          [0  sy  0 ]
         *          [0  0   sz]
         * </pre>
         * and
         * <pre>
         *     Ta = [1          -alphaXy    alphaXz ]
         *          [alphaYx    1           -alphaYz]
         *          [-alphaZx   alphaZy     1       ]
         * </pre>
         * Hence:
         * <pre>
         *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
         *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
         *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
         * </pre>
         * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
         * are considered to be zero if the accelerometer z-axis is assumed to be the same
         * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
         * becomes upper diagonal:
         * <pre>
         *     Ma = [sx    mxy  mxz]
         *          [0     sy   myz]
         *          [0     0    sz ]
         * </pre>
         * Values of this matrix are unitless.
         */
        private Matrix mEstimatedMa;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4677
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6134
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4812
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5999
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5748
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9910
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7065
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8496
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4897
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6426
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5044
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6284
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6020
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10421
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7412
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8925
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator3D.java 489
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator3D.java 504
                                            MSACRobustRangingAndRssiRadioSourceEstimator3D.this, progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point3D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingRadioSourceEstimator2D.java 267
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator2D.java 502
                                            MSACRobustRangingRadioSourceEstimator2D.this,
                                            progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point2D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3199
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4686
        mB = invInitialM.multiplyAndReturnNew(bg);

        mFitter.setFunctionEvaluator(
                new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
                    @Override
                    public int getNumberOfDimensions() {
                        // Before and after normalized gravity versors
                        return 2 * BodyKinematics.COMPONENTS;
                    }

                    @Override
                    public double[] createInitialParametersArray() {
                        final double[] initial =
                                new double[GENERAL_UNKNOWNS_AND_CROSS_BIASES];

                        // cross coupling errors M
                        final int num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                        for (int i = 0; i < num; i++) {
                            initial[i] = initialM.getElementAtIndex(i);
                        }

                        // g-dependent cross biases G
                        for (int i = 0, j = num; i < num; i++, j++) {
                            initial[j] = initialG.getElementAtIndex(i);
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 11959
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 12549
            final double angularRateMeasZ2 = mBiasZ + m1.getElementAtIndex(2);

            final double sqrNormMeas1 = angularRateMeasX1 * angularRateMeasX1
                    + angularRateMeasY1 * angularRateMeasY1
                    + angularRateMeasZ1 * angularRateMeasZ1;
            final double sqrNormMeas2 = angularRateMeasX2 * angularRateMeasX2
                    + angularRateMeasY2 * angularRateMeasY2
                    + angularRateMeasZ2 * angularRateMeasZ2;

            final double normMeas1 = Math.sqrt(sqrNormMeas1);
            final double normMeas2 = Math.sqrt(sqrNormMeas2);

            return Math.abs(normMeas1 - normMeas2);

        } catch (final WrongSizeException | InvalidRotationMatrixException
                | InvalidSourceAndDestinationFrameTypeException e) {
            return Double.MAX_VALUE;
        }
    }

    /**
     * Computes a preliminary solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminary solution will be stored.
     */
    protected void computePreliminarySolutions(
            final int[] samplesIndices,
            final List<PreliminaryResult> solutions) {

        final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();

        for (final int samplesIndex : samplesIndices) {
            measurements.add(mMeasurements.get(samplesIndex));
        }

        try {
            final PreliminaryResult result = new PreliminaryResult();
            result.mEstimatedMg = getInitialMg();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 371
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1134
            setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 371
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1132
            setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7673
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3613
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5149
                        return BodyKinematics.COMPONENTS;
                    }

                    @Override
                    public double[] createInitialParametersArray() {
                        final double[] initial = new double[COMMON_Z_AXIS_UNKNOWNS];

                        // biases b
                        for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                            initial[i] = initialB.getElementAtIndex(i);
                        }

                        // upper diagonal cross coupling errors M
                        int k = BodyKinematics.COMPONENTS;
                        for (int j = 0; j < BodyKinematics.COMPONENTS; j++) {
                            for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(final int i, final double[] point,
                                           final double[] params, final double[] derivatives)
                            throws EvaluationException {
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 15725
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 7487
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15618
                TimeUnit.SECOND);
    }

    /**
     * Converts provided distance instance into its corresponding value expressed in
     * meters.
     *
     * @param distance distance instance to be converted.
     * @return converted value expressed in meters.
     */
    private static double convertDistanceToDouble(final Distance distance) {
        return DistanceConverter.convert(distance.getValue().doubleValue(),
                distance.getUnit(), DistanceUnit.METER);
    }

    /**
     * Converts provided speed instance into its corresponding value expressed in
     * meters per second.
     *
     * @param speed speed instance to be converted.
     * @return converted value expressed in meters per second.
     */
    private static double convertSpeedToDouble(final Speed speed) {
        return SpeedConverter.convert(speed.getValue().doubleValue(),
                speed.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Converts provided acceleration instance into its corresponding value expressed
     * in meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value expressed in meters per squared second.
     */
    private static double convertAccelerationToDouble(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(),
                acceleration.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts provided angular speed into its corresponding value expressed in
     * radians per second.
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value expressed in radians per second.
     */
    private static double convertAngularSpeedToDouble(final AngularSpeed angularSpeed) {
        return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(),
                angularSpeed.getUnit(), AngularSpeedUnit.RADIANS_PER_SECOND);
    }
}
File Line
com/irurueta/navigation/gnss/GNSSKalmanFilteredEstimator.java 458
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1497
    public boolean getState(final GNSSKalmanState result) {
        if (mState != null) {
            result.copyFrom(mState);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets timestamp expressed in seconds since epoch time when Kalman filter state
     * was last propagated.
     *
     * @return timestamp expressed in seconds since epoch time when Kalman filter
     * state was last propagated.
     */
    public Double getLastStateTimestamp() {
        return mLastStateTimestamp;
    }

    /**
     * Gets timestamp since epoch time when Kalman filter state was last propageted.
     *
     * @param result instance where timestamp since epoch time when Kalman filter
     *               state was last propagated will be stored.
     * @return true if result instance is updated, false otherwise.
     */
    public boolean getLastStateTimestampAsTime(final Time result) {
        if (mLastStateTimestamp != null) {
            result.setValue(mLastStateTimestamp);
            result.setUnit(TimeUnit.SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets timestamp since epoch time when Kalman filter state was last propagated.
     *
     * @return timestamp since epoch time when Kalman filter state was last
     * propagated.
     */
    public Time getLastStateTimestampAsTime() {
        return mLastStateTimestamp != null ?
                new Time(mLastStateTimestamp, TimeUnit.SECOND) : null;
    }

    /**
     * Indicates whether this estimator is running or not.
     *
     * @return true if this estimator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Indicates whether provided measurements are ready to
     * be used for an update.
     *
     * @param measurements measurements to be checked.
     * @return true if estimator is ready, false otherwise.
     */
    public static boolean isUpdateMeasurementsReady(
            final Collection<GNSSMeasurement> measurements) {
        return GNSSLeastSquaresPositionAndVelocityEstimator
                .isValidMeasurements(measurements);
    }

    /**
     * Updates GNSS measurements of this estimator when new satellite measurements
     * are available.
     * Calls to this method will be ignored if interval between provided timestamp
     * and last timestamp when Kalman filter was updated is less than epoch interval.
     *
     * @param measurements GNSS measurements to be updated.
     * @param timestamp    timestamp since epoch time when GNSS measurements were
     *                     updated.
     * @return true if measurements were updated, false otherwise.
     * @throws LockedException   if this estimator is already running.
     * @throws NotReadyException if estimator is not ready for measurements updates.
     * @throws GNSSException     if estimation fails due to numerical instabilities.
     */
    public boolean updateMeasurements(
            final Collection<GNSSMeasurement> measurements, final Time timestamp)
            throws LockedException, NotReadyException, GNSSException {
File Line
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator.java 66
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator.java 65
public abstract class SequentialRobustMixedRadioSourceEstimator<S extends RadioSource,
        P extends Point<P>> {

    /**
     * Default robust estimator method for robust position estimation using ranging
     * data when no robust method is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_PANGING_ROBUST_METHOD =
            RobustEstimatorMethod.PROMedS;

    /**
     * Default robust estimator method for pathloss exponent and transmitted power
     * estimation using RSSI data when no robust method is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_RSSI_ROBUST_METHOD =
            RobustEstimatorMethod.PROMedS;

    /**
     * Indicates that result is refined by default using all found inliers.
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen subsamples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Indicates that by default position covariances of readings must be taken into account to increase
     * the amount of standard deviation of each ranging measure by the amount of position standard deviation
     * assuming that both measures are statistically independent.
     */
    public static final boolean DEFAULT_USE_READING_POSITION_COVARIANCES = true;

    /**
     * Internal robust estimator for position estimation.
     */
    protected RobustRangingRadioSourceEstimator<S, P> mRangingEstimator;

    /**
     * Internal robust estimator for pathloss exponent and transmitted power
     * estimation.
     */
    protected RobustRssiRadioSourceEstimator<S, P> mRssiEstimator;

    /**
     * Robust method used for robust position estimation using ranging data.
     */
    protected RobustEstimatorMethod mRangingRobustMethod = DEFAULT_PANGING_ROBUST_METHOD;

    /**
     * Robust method used for pathloss exponent and transmitted power estimation
     * using RSSI data.
     */
    protected RobustEstimatorMethod mRssiRobustMethod = DEFAULT_RSSI_ROBUST_METHOD;

    /**
     * Size of subsets to be checked during ranging robust estimation.
     */
    protected int mRangingPreliminarySubsetSize;

    /**
     * Size of subsets to be checked during RSSI robust estimation.
     */
    protected int mRssiPreliminarySubsetSize;

    /**
     * Threshold to determine when samples are inliers or not used during robust
     * position estimation.
     * If not defined, default threshold will be used.
     */
    protected Double mRangingThreshold;

    /**
     * Threshold to determine when samples are inliers or not used during robust
     * pathloss exponent and transmitted power estimation.
     */
    protected Double mRssiThreshold;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2961
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3938
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final double[] initial = new double[COMMON_Z_AXIS_UNKNOWNS];

                initial[0] = mInitialSx;
                initial[1] = mInitialSy;
                initial[2] = mInitialSz;

                initial[3] = mInitialMxy;
                initial[4] = mInitialMxz;
                initial[5] = mInitialMyz;

                return initial;
            }

            @Override
            public void evaluate(
                    final int i, final double[] point, final double[] result,
                    final double[] params, final Matrix jacobian) {
                // We know that
                //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                //  fmeasy = by + ftruey + sy * ftruey + myz * ftruez
                //  fmeasz = bz + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters sx, sy, sz, mxy, mxz, myz are:

                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myz) = 0.0

                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myz) = ftruez

                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = 0.0
                // d(fmeasy)/d(sz) = ftruez
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myz) = 0.0

                final double sx = params[0];
                final double sy = params[1];
                final double sz = params[2];

                final double mxy = params[3];
                final double mxz = params[4];
                final double myz = params[5];

                final double ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3423
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4091
                initial[3] = mInitialSx;
                initial[4] = mInitialSy;
                initial[5] = mInitialSz;

                initial[6] = mInitialMxy;
                initial[7] = mInitialMxz;
                initial[8] = mInitialMyz;

                return initial;
            }

            @Override
            public void evaluate(
                    final int i, final double[] point, final double[] result,
                    final double[] params, final Matrix jacobian) {
                // We know that:
                //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                //  fmeasy = by + ftruey + sy * ftruey + myz * ftruez
                //  fmeasz = bz + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
                // sz, mxy, mxz, myz

                // d(fmeasx)/d(bx) = 1.0
                // d(fmeasx)/d(by) = 0.0
                // d(fmeasx)/d(bz) = 0.0
                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myz) = 0.0

                // d(fmeasy)/d(bx) = 0.0
                // d(fmeasy)/d(by) = 1.0
                // d(fmeasy)/d(bz) = 0.0
                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myz) = ftruez

                // d(fmeasz)/d(bx) = 0.0
                // d(fmeasz)/d(by) = 0.0
                // d(fmeasz)/d(bz) = 1.0
                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myz) = 0.0

                final double bx = params[0];
                final double by = params[1];
                final double bz = params[2];

                final double sx = params[3];
                final double sy = params[4];
                final double sz = params[5];

                final double mxy = params[6];
                final double mxz = params[7];
                final double myz = params[8];

                final double ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7903
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5887
            mFmeas.setElementAtIndex(0, mFmeasX);
            mFmeas.setElementAtIndex(1, mFmeasY);
            mFmeas.setElementAtIndex(2, mFmeasZ);

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);

            mB.setElementAtIndex(0, bx);
            mB.setElementAtIndex(1, by);
            mB.setElementAtIndex(2, bz);
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingRadioSourceEstimator2D.java 308
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingRadioSourceEstimator2D.java 267
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator2D.java 502
                                    mListener.onEstimateProgressChange(LMedSRobustRangingRadioSourceEstimator2D.this,
                                            progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point2D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingRadioSourceEstimator3D.java 309
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator3D.java 489
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator3D.java 504
                                    mListener.onEstimateProgressChange(LMedSRobustRangingRadioSourceEstimator3D.this,
                                            progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point3D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1028
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 370
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1035
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 384
    }

    /**
     * Indicates whether estimator is ready to start the estimator.
     *
     * @return true if estimator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null
                && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is currently running or not.
     *
     * @return true if estimator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if estimator is currently running.
     * @throws NotReadyException    if estimator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unitless.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 639
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1864
        super(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<PreliminaryResult> innerEstimator =
                new MSACRobustEstimator<>(
                        new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 624
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1834
        super(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, bias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<PreliminaryResult> innerEstimator =
                new MSACRobustEstimator<>(
                        new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3664
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3779
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must be 3x1 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must be 3x1
     *                          and is expressed in meters per squared
     *                          second (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3458
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3573
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must be 3x1 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must be 3x1
     *                          and is expressed in meters per squared
     *                          second (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4961
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7664
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6283
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9069
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7280
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8837
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7427
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8688
            final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, bias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5201
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8047
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6583
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9534
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7644
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9288
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7798
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9132
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        qualityScores, position, turntableRotationRate,
                        timeInterval, measurements, initialBias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 580
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1134
            setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 581
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1132
            setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2286
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 371
            initialize();

            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2284
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 371
            initialize();

            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/lateration/LMedSRobustLateration2DSolver.java 332
com/irurueta/navigation/lateration/MSACRobustLateration2DSolver.java 287
                                    LMedSRobustLateration2DSolver.this, progress);
                        }
                    }
                });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onSolveStart(this);
            }

            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            Point2D result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (mListener != null) {
                mListener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/lateration/LMedSRobustLateration3DSolver.java 334
com/irurueta/navigation/lateration/MSACRobustLateration3DSolver.java 288
                                    LMedSRobustLateration3DSolver.this, progress);
                        }
                    }
                });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onSolveStart(this);
            }

            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            Point3D result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (mListener != null) {
                mListener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator2D.java 546
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator2D.java 489
                                            LMedSRobustRssiRadioSourceEstimator2D.this,
                                            progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            Solution<Point2D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator3D.java 545
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingRadioSourceEstimator3D.java 265
                                            LMedSRobustRssiRadioSourceEstimator3D.this, progress);
                                }
                            }
                        });

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            Solution<Point3D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 331
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 330
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 860
        super(readings, initialPosition, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mReadings.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2660
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 363
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2927
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 377
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3186
            final KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException |
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 7048
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7687
                        for (int j = 0; j < BodyKinematics.COMPONENTS; j++) {
                            for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(final int i, final double[] point,
                                           final double[] params, final double[] derivatives)
                            throws EvaluationException {

                        mFmeasX = point[0];
                        mFmeasY = point[1];
                        mFmeasZ = point[2];

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateCommonAxis(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double m11 = result[0];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4888
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5163
                        for (int j = 0; j < BodyKinematics.COMPONENTS; j++) {
                            for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {

                        mMeasAngularRateX = point[0];
                        mMeasAngularRateY = point[1];
                        mMeasAngularRateZ = point[2];

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateCommonAxis(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double m11 = result[0];
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2537
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2681
                        for (int j = 0; j < BodyMagneticFluxDensity.COMPONENTS; j++) {
                            for (int i = 0; i < BodyMagneticFluxDensity.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {

                        mBmeasX = point[0];
                        mBmeasY = point[1];
                        mBmeasZ = point[2];

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateCommonAxis(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double m11 = result[0];
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 566
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 562
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2270
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2269
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 589
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 590
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2295
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2293
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 1725
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 7755
                        new double[]{alphaX, alphaY, alphaZ});

                if (alphaNorm > ALPHA_THRESHOLD) {
                    final double alphaNorm2 = alphaNorm * alphaNorm;
                    final double value1 = (1.0 - Math.cos(alphaNorm)) / alphaNorm2;
                    final double value2 = (1.0 - Math.sin(alphaNorm) / alphaNorm) / alphaNorm2;
                    final Matrix tmp1 = alphaSkew1.multiplyByScalarAndReturnNew(value1);
                    final Matrix tmp2 = alphaSkew1.multiplyByScalarAndReturnNew(value2);
                    tmp2.multiply(alphaSkew1);

                    final Matrix tmp3 = Matrix.identity(ROWS, ROWS);
                    tmp3.add(tmp1);
                    tmp3.add(tmp2);

                    oldCbe.multiply(tmp3);
                }

                final Matrix alphaSkew2 = Utils.skewMatrix(
                        new double[]{0.0, 0.0, alpha});
                alphaSkew2.multiplyByScalar(0.5);
                alphaSkew2.multiply(oldCbe); // 0.5 * alphaSkew2 * oldCbe

                oldCbe.subtract(alphaSkew2); // oldCbe - 0.5 * alphaSkew2 * oldCbe
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7543
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5342
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2532
                        mFmeasZ = point[2];

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateGeneral(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];
        final double m21 = result[4];
        final double m31 = result[5];

        final double m12 = result[6];
        final double m22 = result[7];
        final double m32 = result[8];

        final double m13 = result[9];
        final double m23 = result[10];
        final double m33 = result[11];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4608
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5359
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias,
            final Matrix initialMa,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        bias, initialMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        bias, initialMa, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  list of body kinematics measurements taken at a given position with
     *                      different unknown orientations and containing the standard deviations
     *                      of accelerometer and gyroscope measurements.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4831
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5603
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias,
            final Matrix initialMa,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, initialMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, initialMa, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  list of body kinematics measurements taken at a given position with
     *                      different unknown orientations and containing the standard deviations
     *                      of accelerometer and gyroscope measurements.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 13 samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3678
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5261
                angularSpeed.getUnit(), AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Makes proper conversion of internal cross-coupling, bias and g-dependent
     * cross bias matrices.
     *
     * @param m internal scaling and cross-coupling matrix.
     * @param g internal g-dependent cross bias matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m, final Matrix g)
            throws AlgebraException {
        setResult(m);

        // Gg = M*G
        m.multiply(g, mEstimatedGg);
    }

    /**
     * Makes proper conversion of internal cross-coupling and bias matrices.
     *
     * @param m internal scaling and cross-coupling matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m) throws AlgebraException {
        // Because:
        // M = I + Mg
        // b = M^-1*bg

        // Then:
        // Mg = M - I
        // bg = M*b

        if (mEstimatedMg == null) {
            mEstimatedMg = m;
        } else {
            mEstimatedMg.copyFrom(m);
        }

        for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
            mEstimatedMg.setElementAt(i, i,
                    mEstimatedMg.getElementAt(i, i) - 1.0);
        }

        if (mEstimatedGg == null) {
            mEstimatedGg = new Matrix(
                    BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            mEstimatedGg.initialize(0.0);
        }

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Computes gravity versor error at the end of a sequence using provided
     * parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param i      row position.
     * @param params array containing parameters for the general purpose case
     *               when G-dependent cross biases are taken into account. Must
     *               have length 18.
     * @return error between estimated and measured gravity versor.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneralWithGDependentCrossBiases(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3450
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3546
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences   collection of sequences containing timestamped body
     *                    kinematics measurements.
     * @param initialBias initial gyroscope bias to be used to find a
     *                    solution. This must have length 3 and is expressed
     *                    in radians per second (rad/s).
     * @param initialMg   initial gyroscope scale factors and cross coupling
     *                    errors matrix. Must be 3x3.
     * @param initialGg   initial gyroscope G-dependent cross biases
     *                    introduced on the gyroscope by the specific forces
     *                    sensed by the accelerometer. Must be 3x3.
     * @param method      robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3836
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3958
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3244
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3340
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences   collection of sequences containing timestamped body
     *                    kinematics measurements.
     * @param initialBias initial gyroscope bias to be used to find a
     *                    solution. This must have length 3 and is expressed
     *                    in radians per second (rad/s).
     * @param initialMg   initial gyroscope scale factors and cross coupling
     *                    errors matrix. Must be 3x3.
     * @param initialGg   initial gyroscope G-dependent cross biases
     *                    introduced on the gyroscope by the specific forces
     *                    sensed by the accelerometer. Must be 3x3.
     * @param method      robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3630
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3752
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4961
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9069
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5460
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8198
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6283
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7664
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6782
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9606
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5201
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9534
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5723
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8606
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6583
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8047
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7105
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10096
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias, initialMg,
                        initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias, initialMg,
                        initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias, initialMg,
                        initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5279
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 6126
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron,
            final Matrix initialMm,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, initialMm, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, initialMm, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5388
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6235
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final Matrix initialMm,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, initialMm, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, initialMm, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
File Line
com/irurueta/navigation/indoor/position/NonLinearMixedPositionEstimator.java 352
com/irurueta/navigation/indoor/position/NonLinearRangingAndRssiPositionEstimator.java 352
com/irurueta/navigation/indoor/position/NonLinearRangingPositionEstimator.java 351
com/irurueta/navigation/indoor/position/NonLinearRssiPositionEstimator.java 350
                            NonLinearMixedPositionEstimator.this);
                }
            }
        };
    }

    /**
     * Builds positions, distances and standard deviation of distances for the internal
     * lateration solver.
     */
    @SuppressWarnings("Duplicates")
    private void buildPositionsDistancesAndDistanceStandardDeviations() {
        if (mTrilaterationSolver == null) {
            return;
        }

        final int min = getMinRequiredSources();
        if (mSources == null || mFingerprint == null ||
                mSources.size() < min ||
                mFingerprint.getReadings() == null ||
                mFingerprint.getReadings().size() < min) {
            return;
        }

        final List<P> positions = new ArrayList<>();
        final List<Double> distances = new ArrayList<>();
        final List<Double> distanceStandardDeviations = new ArrayList<>();
        PositionEstimatorHelper.buildPositionsDistancesAndDistanceStandardDeviations(
                mSources, mFingerprint, mUseRadioSourcePositionCovariance,
                mFallbackDistanceStandardDeviation, positions, distances,
                distanceStandardDeviations);

        setPositionsDistancesAndDistanceStandardDeviations(positions, distances,
                distanceStandardDeviations);
    }
}
File Line
com/irurueta/navigation/indoor/position/NonLinearMixedPositionEstimator3D.java 324
com/irurueta/navigation/indoor/position/NonLinearRangingAndRssiPositionEstimator3D.java 324
com/irurueta/navigation/indoor/position/NonLinearRangingPositionEstimator3D.java 324
com/irurueta/navigation/indoor/position/NonLinearRssiPositionEstimator3D.java 325
            final List<Point3D> positions, List<Double> distances,
            final List<Double> distanceStandardDeviations) {

        final int size = positions.size();
        Point3D[] positionsArray = new InhomogeneousPoint3D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];
        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);
        }

        try {
            mTrilaterationSolver.setPositionsDistancesAndStandardDeviations(
                    positionsArray, distancesArray, distanceStandardDeviationsArray);
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }

    /**
     * Initializes lateration solver.
     */
    private void init() {
        mTrilaterationSolver = new NonLinearLeastSquaresLateration3DSolver(
                mLaterationSolverListener);
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator2D.java 935
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator2D.java 917
                    beacon.getTransmittedPower(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(),
                    getEstimatedPathLossExponent(),
                    transmittedPowerStandardDeviation,
                    pathlossExponentStandardDeviation,
                    estimatedPosition, estimatedPositionCovariance);
        } else {
            return null;
        }
    }

    /**
     * Builds ranging estimator.
     */
    @Override
    protected void buildRangingEstimatorIfNeeded() {
        if (mRangingEstimator == null || mRangingEstimator.getMethod() != mRangingRobustMethod) {
            mRangingEstimator = RobustRangingRadioSourceEstimator2D.create(mRangingRobustMethod);
        }
    }

    /**
     * build RSSI estimator.
     *
     * @throws LockedException if estimator is locked.
     */
    @Override
    protected void buildRssiEstimatorIfNeeded() throws LockedException {
        if (mRssiEstimator == null || mRssiEstimator.getMethod() != mRssiRobustMethod) {
            mRssiEstimator = RobustRssiRadioSourceEstimator2D.create(mRssiRobustMethod);

            // rssi estimator will never need position estimator, but to
            // ensure it is ready we need to provide an initial position
            mRssiEstimator.setPositionEstimationEnabled(false);
            mRssiEstimator.setInitialPosition(Point2D.create());
        }
    }
File Line
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator3D.java 935
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator3D.java 915
                    beacon.getTransmittedPower(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(),
                    getEstimatedPathLossExponent(),
                    transmittedPowerStandardDeviation,
                    pathlossExponentStandardDeviation,
                    estimatedPosition, estimatedPositionCovariance);
        } else {
            return null;
        }
    }

    /**
     * Builds ranging estimator.
     */
    @Override
    protected void buildRangingEstimatorIfNeeded() {
        if (mRangingEstimator == null || mRangingEstimator.getMethod() != mRangingRobustMethod) {
            mRangingEstimator = RobustRangingRadioSourceEstimator3D.create(mRangingRobustMethod);
        }
    }

    /**
     * Build RSSI estimator.
     *
     * @throws LockedException if estimator is locked.
     */
    @Override
    protected void buildRssiEstimatorIfNeeded() throws LockedException {
        if (mRssiEstimator == null || mRssiEstimator.getMethod() != mRssiRobustMethod) {
            mRssiEstimator = RobustRssiRadioSourceEstimator3D.create(mRssiRobustMethod);

            // rssi estimator will never need position estimator, but to
            // ensure it is ready we need to provide an initial position
            mRssiEstimator.setPositionEstimationEnabled(false);
            mRssiEstimator.setInitialPosition(Point3D.create());
        }
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1778
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 554
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1789
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 562
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1805
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 582
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1812
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 589
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4628
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4763
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores     quality scores corresponding to each provided
     *                          sequence. The larger the score value the better
     *                          the quality of the sequence.
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must have length 3 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must
     *                          have length 3 and is expressed in
     *                          meters per squared second
     *                          (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param listener          listener to handle events raised by this
     *                          calibrator.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4422
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4557
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores     quality scores corresponding to each provided
     *                          sequence. The larger the score value the better
     *                          the quality of the sequence.
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must have length 3 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must
     *                          have length 3 and is expressed in
     *                          meters per squared second
     *                          (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param listener          listener to handle events raised by this
     *                          calibrator.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5121
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7835
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5289
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8015
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5460
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9606
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6443
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9243
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6611
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9423
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6782
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8198
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5364
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8224
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5540
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8413
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5723
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10096
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6746
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9711
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6922
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9901
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7105
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8606
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias, initialMg,
                        initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias, initialMg,
                        initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias, initialMg,
                        initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/gnss/GNSSEstimation.java 707
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 1573
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 1747
                clockDrift.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets estimated ECEF user position expressed in meters (m).
     *
     * @param result instance where estimated ECEF user position will be stored.
     */
    public void getPosition(final Point3D result) {
        result.setInhomogeneousCoordinates(mX, mY, mZ);
    }

    /**
     * Gets estimated ECEF user position expressed in meters (m).
     *
     * @return estimated ECEF user position.
     */
    public Point3D getPosition() {
        return new InhomogeneousPoint3D(mX, mY, mZ);
    }

    /**
     * Sets estimated ECEF user position expressed in meters (m).
     *
     * @param position estimated ECEF user position.
     */
    public void setPosition(final Point3D position) {
        mX = position.getInhomX();
        mY = position.getInhomY();
        mZ = position.getInhomZ();
    }

    /**
     * Gets estimatedECEF user position.
     *
     * @param result instance where result will be stored.
     */
    public void getEcefPosition(final ECEFPosition result) {
        result.setCoordinates(mX, mY, mZ);
    }

    /**
     * Gets estimated ECEF user position.
     *
     * @return estimated ECEF user position.
     */
    public ECEFPosition getEcefPosition() {
        return new ECEFPosition(mX, mY, mZ);
    }

    /**
     * Sets estimated ECEF user position.
     *
     * @param ecefPosition estimated ECEF user position.
     */
    public void setEcefPosition(final ECEFPosition ecefPosition) {
        mX = ecefPosition.getX();
        mY = ecefPosition.getY();
        mZ = ecefPosition.getZ();
    }

    /**
     * Gets estimated ECEF user velocity.
     *
     * @param result instance where result will be stored.
     */
    public void getEcefVelocity(final ECEFVelocity result) {
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator.java 521
com/irurueta/navigation/indoor/radiosource/RangingAndRssiRadioSourceEstimator.java 516
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator.java 473
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator.java 469
com/irurueta/navigation/indoor/radiosource/RssiRadioSourceEstimator.java 518
            final MixedRadioSourceEstimatorListener<S, P> listener) {
        this(readings, initialPosition, initialTransmittedPowerdBm, listener);
        mInitialPathLossExponent = initialPathLossExponent;
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPowerdBm() {
        return mInitialTransmittedPowerdBm;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @param initialTransmittedPowerdBm initial transmitted power to start the
     *                                   estimation of radio source transmitted
     *                                   power.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialTransmittedPowerdBm(final Double initialTransmittedPowerdBm)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialTransmittedPowerdBm = initialTransmittedPowerdBm;
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPower() {
        return mInitialTransmittedPowerdBm != null ?
                Utils.dBmToPower(mInitialTransmittedPowerdBm) : null;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @param initialTransmittedPower initial transmitted power to start the
     *                                estimation of radio source transmitted power.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setInitialTransmittedPower(final Double initialTransmittedPower)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (initialTransmittedPower != null) {
            if (initialTransmittedPower < 0.0) {
                throw new IllegalArgumentException();
            }
            mInitialTransmittedPowerdBm = Utils.powerTodBm(
                    initialTransmittedPower);
        } else {
            mInitialTransmittedPowerdBm = null;
        }
    }

    /**
     * Indicates whether transmitted power estimation is enabled or not.
     *
     * @return true if transmitted power estimation is enabled, false otherwise.
     */
    public boolean isTransmittedPowerEstimationEnabled() {
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator2D.java 3025
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator3D.java 3032
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator2D.java 3007
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator3D.java 3008
                    if (mTransmittedPowerEstimationEnabled) {
                        //transmitted power estimation enabled
                        mEstimatedTransmittedPowerVariance = mCovariance.
                                getElementAt(pos, pos);
                        pos++;
                    } else {
                        //transmitted power estimation disabled
                        mEstimatedTransmittedPowerVariance = null;
                    }

                    if (mPathLossEstimationEnabled) {
                        //pathloss exponent estimation enabled
                        mEstimatedPathLossExponentVariance = mCovariance.
                                getElementAt(pos, pos);
                    } else {
                        //pathloss exponent estimation disabled
                        mEstimatedPathLossExponentVariance = null;
                    }
                } else {
                    mCovariance = null;
                    mEstimatedPositionCovariance = null;
                    mEstimatedTransmittedPowerVariance = null;
                    mEstimatedPathLossExponentVariance = null;
                }

                mEstimatedPosition = mInnerEstimator.getEstimatedPosition();
                mEstimatedTransmittedPowerdBm =
                        mInnerEstimator.getEstimatedTransmittedPowerdBm();
                mEstimatedPathLossExponent =
                        mInnerEstimator.getEstimatedPathLossExponent();
            } catch (final Exception e) {
                //refinement failed, so we return input value, and covariance
                //becomes unavailable
                mCovariance = null;
                mEstimatedPositionCovariance = null;
                mEstimatedTransmittedPowerVariance = null;
                mEstimatedPathLossExponentVariance = null;

                mEstimatedPosition = initialPosition;
                mEstimatedTransmittedPowerdBm = initialTransmittedPowerdBm;
                mEstimatedPathLossExponent = initialPathLossExponent;
            }
        } else {
            mCovariance = null;
            mEstimatedPositionCovariance = null;
            mEstimatedTransmittedPowerVariance = null;
            mEstimatedPathLossExponentVariance = null;

            mEstimatedPosition = initialPosition;
            mEstimatedTransmittedPowerdBm = initialTransmittedPowerdBm;
            mEstimatedPathLossExponent = initialPathLossExponent;
        }
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 682
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 690
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2701
            final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Gets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return mBiasX;
    }

    /**
     * Sets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasX x coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
    }

    /**
     * Gets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return y coordinate of accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return mBiasY;
    }

    /**
     * Sets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasY y coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasY = biasY;
    }

    /**
     * Gets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return z coordinate of accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return mBiasZ;
    }

    /**
     * Sets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasZ = biasZ;
    }

    /**
     * Gets known x coordinate of accelerometer bias.
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 7205
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7876
    private double evaluate(final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33)
            throws EvaluationException {

        // fmeas = M*(ftrue + b)

        // ftrue = M^-1*fmeas - b

        try {
            if (mFmeas == null) {
                mFmeas = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (mM == null) {
                mM = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }
            if (mInvM == null) {
                mInvM = new Matrix(BodyKinematics.COMPONENTS,
                        BodyKinematics.COMPONENTS);
            }
            if (mB == null) {
                mB = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (mFtrue == null) {
                mFtrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3199
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3861
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias,
            final Matrix initialMa,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position position where body kinematics measures have been taken.
     * @param method   robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final NEDPosition position, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3390
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4061
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias,
            final Matrix initialMa,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position position where body kinematics measures have been taken.
     * @param method   robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final NEDPosition position, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4256
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3953
                mB = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (mG == null) {
                mG = new Matrix(BodyKinematics.COMPONENTS,
                        BodyKinematics.COMPONENTS);
            }
            if (mTmp == null) {
                mTmp = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2696
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2873
            final double m11, final double m21, final double m31,
            final double m12, final double m22, final double m32,
            final double m13, final double m23, final double m33)
            throws EvaluationException {

        // bmeas = M*(btrue + b)

        // btrue = M^-1*bmeas - b

        try {
            if (mBmeas == null) {
                mBmeas = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                        1);
            }
            if (mM == null) {
                mM = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                        BodyMagneticFluxDensity.COMPONENTS);
            }
            if (mInvM == null) {
                mInvM = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                        BodyMagneticFluxDensity.COMPONENTS);
            }
            if (mB == null) {
                mB = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                        1);
            }
            if (mBtrue == null) {
                mBtrue = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                        1);
            }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3464
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4203
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron,
            final Matrix initialMm,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position position where body magnetic flux density measurements
     *                 have been taken.
     * @param method   robust estimator method.
     * @return a robust magnetometer calibrator.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3573
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4312
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final Matrix initialMm,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position position where body magnetic flux density measurements
     *                 have been taken.
     * @param method   robust estimator method.
     * @return a robust magnetometer calibrator.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/indoor/fingerprint/FirstOrderNonLinearFingerprintPositionEstimator2D.java 162
com/irurueta/navigation/indoor/fingerprint/SecondOrderNonLinearFingerprintPositionEstimator2D.java 162
com/irurueta/navigation/indoor/fingerprint/ThirdOrderNonLinearFingerprintPositionEstimator2D.java 162
    }

    /**
     * Evaluates a non-linear multi dimension function at provided point using
     * provided parameters and returns its evaluation and derivatives of the
     * function respect the function parameters.
     *
     * @param i           number of sample being evaluated.
     * @param point       point where function will be evaluated.
     * @param params      initial parameters estimation to be tried. These will
     *                    change as the Levenberg-Marquardt algorithm iterates to the best solution.
     *                    These are used as input parameters along with point to evaluate function.
     * @param derivatives partial derivatives of the function respect to each
     *                    provided parameter.
     * @return function evaluation at provided point.
     */
    @Override
    @SuppressWarnings("Duplicates")
    protected double evaluate(
            final int i, final double[] point, final double[] params,
            final double[] derivatives) {
        //This method implements received power at point pi = (xi, yi) and its derivatives

        //Pr(pi) = Pr(p1)
        //  - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
        //  - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)

        final double xi = params[0];
        final double yi = params[1];

        //received power
        final double pr = point[0];

        //fingerprint coordinates
        final double x1 = point[1];
        final double y1 = point[2];

        //radio source coordinates
        final double xa = point[3];
        final double ya = point[4];

        //path loss exponent
        final double n = point[5];

        final double ln10 = Math.log(10.0);

        final double diffXi1 = xi - x1;
        final double diffYi1 = yi - y1;

        final double diffX1a = x1 - xa;
        final double diffY1a = y1 - ya;

        final double diffX1a2 = diffX1a * diffX1a;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 640
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1835
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<PreliminaryResult> innerEstimator =
                new MSACRobustEstimator<>(
                        new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 625
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1865
                estimateGDependentCrossBiases, bias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<PreliminaryResult> innerEstimator =
                new MSACRobustEstimator<>(
                        new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5121
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9243
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5289
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9423
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6443
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7835
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6611
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8015
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5364
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9711
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5540
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9901
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6746
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8224
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6922
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8413
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/lateration/NonLinearLeastSquaresLateration2DSolver.java 306
com/irurueta/navigation/lateration/RobustLateration2DSolver.java 248
    }

    /**
     * Gets circles defined by provided positions and distances.
     *
     * @return circles defined by provided positions and distances.
     */
    public Circle[] getCircles() {
        if (mPositions == null) {
            return null;
        }

        final Circle[] result = new Circle[mPositions.length];

        for (int i = 0; i < mPositions.length; i++) {
            result[i] = new Circle(mPositions[i], mDistances[i]);
        }
        return result;
    }

    /**
     * Sets circles defining positions and euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 2.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setCircles(final Circle[] circles) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetCircles(circles);
    }

    /**
     * Sets circles defining positions and euclidean distances along with the standard
     * deviations of provided circles radii.
     *
     * @param circles                  circles defining positions and distances.
     * @param radiusStandardDeviations standard deviations of circles radii.
     * @throws IllegalArgumentException if circles is null, length of arrays is less than
     *                                  2 or don't have the same length.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setCirclesAndStandardDeviations(
            final Circle[] circles, final double[] radiusStandardDeviations)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetCirclesAndStandardDeviations(circles, radiusStandardDeviations);
    }
File Line
com/irurueta/navigation/lateration/NonLinearLeastSquaresLateration3DSolver.java 306
com/irurueta/navigation/lateration/RobustLateration3DSolver.java 250
    }

    /**
     * Gets spheres defined by provided positions and distances.
     *
     * @return spheres defined by provided positions and distances.
     */
    public Sphere[] getSpheres() {
        if (mPositions == null) {
            return null;
        }

        final Sphere[] result = new Sphere[mPositions.length];

        for (int i = 0; i < mPositions.length; i++) {
            result[i] = new Sphere(mPositions[i], mDistances[i]);
        }
        return result;
    }

    /**
     * Sets spheres defining positions and euclidean distances.
     *
     * @param spheres spheres defining positions and distances.
     * @throws IllegalArgumentException if spheres is null or length of array of spheres
     *                                  is less than 2.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setSpheres(final Sphere[] spheres) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSpheres(spheres);
    }

    /**
     * Sets spheres defining positions and euclidean distances along with the standard
     * deviations of provided spheres radii.
     *
     * @param spheres                  spheres defining positions and distances.
     * @param radiusStandardDeviations standard deviations of circles radii.
     * @throws IllegalArgumentException if spheres is null, length of arrays is less than
     *                                  2 or don't have the same length.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setSpheresAndStandardDeviations(
            final Sphere[] spheres, final double[] radiusStandardDeviations)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSpheresAndStandardDeviations(spheres, radiusStandardDeviations);
    }
File Line
com/irurueta/navigation/indoor/position/RobustMixedPositionEstimator2D.java 994
com/irurueta/navigation/indoor/position/RobustMixedPositionEstimator3D.java 998
com/irurueta/navigation/indoor/position/RobustRangingAndRssiPositionEstimator3D.java 990
com/irurueta/navigation/indoor/position/RobustRangingPositionEstimator3D.java 999
com/irurueta/navigation/indoor/position/RobustRssiPositionEstimator3D.java 990
        Point2D[] positionsArray = new InhomogeneousPoint2D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];

        double[] qualityScoresArray = null;
        if (distanceQualityScores != null) {
            qualityScoresArray = new double[size];
        }

        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);

            if (qualityScoresArray != null) {
                qualityScoresArray[i] = distanceQualityScores.get(i);
            }
        }

        try {
            mLaterationSolver.setPositionsDistancesAndStandardDeviations(
                    positionsArray, distancesArray, distanceStandardDeviationsArray);

            if (qualityScoresArray != null) {
                mLaterationSolver.setQualityScores(qualityScoresArray);
            }
        } catch (LockedException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4229
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4978
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4437
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5207
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4152
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5260
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5973
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 2775
        final Matrix mg = preliminaryResult.mEstimatedMg;

        final Matrix gg = preliminaryResult.mEstimatedGg;

        try {
            final Matrix m1 = Matrix.identity(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            m1.add(mg);

            final Matrix angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            angularRateTrue.setElementAtIndex(0, angularRateTrueX);
            angularRateTrue.setElementAtIndex(1, angularRateTrueY);
            angularRateTrue.setElementAtIndex(2, angularRateTrueZ);

            m1.multiply(angularRateTrue);

            final Matrix fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            fTrue.setElementAtIndex(0, fTrueX);
            fTrue.setElementAtIndex(1, fTrueY);
            fTrue.setElementAtIndex(2, fTrueZ);
            final Matrix m2 = gg.multiplyAndReturnNew(fTrue);

            m1.add(m2);

            final double angularRateMeasX2 = mBiasX + m1.getElementAtIndex(0);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3946
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5054
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 332
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1109
            setupWmmEstimator();

            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            innerEstimator.setStopThreshold(mStopThreshold);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 348
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1106
            setupWmmEstimator();

            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            innerEstimator.setStopThreshold(mStopThreshold);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4859
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5706
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4968
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5815
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/estimators/NEDPositionEstimator.java 2292
com/irurueta/navigation/inertial/estimators/NEDVelocityEstimator.java 2393
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15604
    private static double convertTimeToDouble(final Time time) {
        return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(),
                TimeUnit.SECOND);
    }

    /**
     * Converts provided angle instance to radians (rad).
     *
     * @param angle angle instance to be converted.
     * @return provided angle value expressed in radians.
     */
    private static double convertAngleToDouble(final Angle angle) {
        return AngleConverter.convert(angle.getValue().doubleValue(), angle.getUnit(), AngleUnit.RADIANS);
    }

    /**
     * Converts provided distance instance to meters (m).
     *
     * @param distance distance instance to be converted.
     * @return provided distance value expressed in meters.
     */
    private static double convertDistanceToDouble(final Distance distance) {
        return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(),
                DistanceUnit.METER);
    }

    /**
     * Converts provided speed instance to meters per second (m/s).
     *
     * @param speed speed instance to be converted.
     * @return provided speed value expressed in meters per second.
     */
    private static double convertSpeedToDouble(final Speed speed) {
        return SpeedConverter.convert(speed.getValue().doubleValue(), speed.getUnit(),
                SpeedUnit.METERS_PER_SECOND);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1350
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3908
        super(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1318
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3845
        super(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, bias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1346
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3904
        super(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @param qualityScores quality scores corresponding to each sequence.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1314
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3841
        super(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, bias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @param qualityScores quality scores corresponding to each sequence.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mSequences.size();
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator3D.java 2871
com/irurueta/navigation/indoor/radiosource/RssiRadioSourceEstimator3D.java 443
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator3D.java 879
        final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings = getReadings();
        if (readings == null || readings.isEmpty()) {
            return null;
        }
        final S source = readings.get(0).getSource();

        final Point3D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        final Double transmittedPowerVariance =
                getEstimatedTransmittedPowerVariance();
        final Double transmittedPowerStandardDeviation = transmittedPowerVariance != null ?
                Math.sqrt(transmittedPowerVariance) : null;

        final Double pathlossExponentVariance =
                getEstimatedPathLossExponentVariance();
        final Double pathlossExponentStandardDeviation = pathlossExponentVariance != null ?
                Math.sqrt(pathlossExponentVariance) : null;

        if (source instanceof WifiAccessPoint) {
            final WifiAccessPoint accessPoint = (WifiAccessPoint) source;
            return new WifiAccessPointWithPowerAndLocated3D(accessPoint.getBssid(),
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 428
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3252
        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 968
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 1049
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1125
            b.setElementAtIndex(i, fMeasZ - fTrueZ);
            i++;
        }

        final Matrix unknowns = Utils.solve(a, b);

        final double bx = unknowns.getElementAtIndex(0);
        final double by = unknowns.getElementAtIndex(1);
        final double bz = unknowns.getElementAtIndex(2);
        final double sx = unknowns.getElementAtIndex(3);
        final double sy = unknowns.getElementAtIndex(4);
        final double sz = unknowns.getElementAtIndex(5);
        final double mxy = unknowns.getElementAtIndex(6);
        final double mxz = unknowns.getElementAtIndex(7);
        final double myx = unknowns.getElementAtIndex(8);
        final double myz = unknowns.getElementAtIndex(9);
        final double mzx = unknowns.getElementAtIndex(10);
        final double mzy = unknowns.getElementAtIndex(11);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2992
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 443
                com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4231
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4414
            final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4412
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5162
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias,
                        listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param initialMa     initial scale factors and cross coupling errors matrix.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4506
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5256
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final Matrix initialMa,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, initialMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, initialMa,
                        listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4557
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5307
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias,
            final Matrix initialMa, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        bias, initialMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        bias, initialMa);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4980
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5164
            final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4439
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4629
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4627
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5398
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param initialMa     initial scale factors and cross coupling errors matrix.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4726
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5497
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMa,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        initialMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        initialMa, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4777
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5548
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias,
            final Matrix initialMa, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, initialMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, initialMa);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5209
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5400
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 11934
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 12524
            final Matrix mg = preliminaryResult.mEstimatedMg;

            final Matrix gg = preliminaryResult.mEstimatedGg;

            final Matrix m1 = Matrix.identity(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            m1.add(mg);

            final Matrix angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            angularRateTrue.setElementAtIndex(0, angularRateTrueX);
            angularRateTrue.setElementAtIndex(1, angularRateTrueY);
            angularRateTrue.setElementAtIndex(2, angularRateTrueZ);

            m1.multiply(angularRateTrue);

            final Matrix fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            fTrue.setElementAtIndex(0, fTrueX);
            fTrue.setElementAtIndex(1, fTrueY);
            fTrue.setElementAtIndex(2, fTrueZ);
            final Matrix m2 = gg.multiplyAndReturnNew(fTrue);

            m1.add(m2);

            final double angularRateMeasX2 = mBiasX + m1.getElementAtIndex(0);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4861
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5059
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5057
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5904
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param initialMm     initial soft-iron matrix containing scale factors
     *                      and cross coupling errors.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5164
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 6011
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron, final Matrix initialMm,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm,
                        listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm,
                        listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        initialMm, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        initialMm, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param initialMm      initial soft-iron matrix containing scale factors
     *                       and cross coupling errors.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5221
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 6068
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron,
            final Matrix initialMm,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, initialMm);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, initialMm);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param initialMm      initial soft-iron matrix containing scale factors
     *                       and cross coupling errors.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5708
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5906
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4970
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5168
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5166
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6013
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5273
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6120
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron, final Matrix initialMm,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        initialMm, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        initialMm, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5330
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6177
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final Matrix initialMm,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, initialMm);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, initialMm);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5817
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6015
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/lateration/PROMedSRobustLateration2DSolver.java 408
com/irurueta/navigation/lateration/PROMedSRobustLateration3DSolver.java 410
        super(circles, distanceStandardDeviations, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mDistances.length;
    }

    /**
     * Solves the lateration problem.
     *
     * @return estimated position.
     * @throws LockedException          if instance is busy solving the lateration problem.
     * @throws NotReadyException        is solver is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public Point2D solve() throws LockedException, NotReadyException,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1292
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 927
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double fMeasX = measuredKinematics.getFx();
            final double fMeasY = measuredKinematics.getFy();
            final double fMeasZ = measuredKinematics.getFz();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, fTrueX);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1403
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 818
        final Matrix a = new Matrix(rows, GENERAL_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double fMeasX = measuredKinematics.getFx();
            final double fMeasY = measuredKinematics.getFy();
            final double fMeasZ = measuredKinematics.getFz();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, fTrueX);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4156
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4497
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ,
                        commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ,
                        commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param biasX         known x coordinate of accelerometer bias.
     * @param biasY         known y coordinate of accelerometer bias.
     * @param biasZ         known z coordinate of accelerometer bias.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores,
            final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4232
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4578
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ,
                        commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ,
                        commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param biasX         known x coordinate of gyroscope bias.
     * @param biasY         known y coordinate of gyroscope bias.
     * @param biasZ         known z coordinate of gyroscope bias.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores, final AngularSpeed biasX,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4609
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6066
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4744
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5931
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4881
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7578
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6203
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8986
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4828
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6357
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4970
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6215
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5119
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7959
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6501
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9446
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/indoor/position/LinearMixedPositionEstimator.java 251
com/irurueta/navigation/indoor/position/LinearRangingAndRssiPositionEstimator.java 252
com/irurueta/navigation/indoor/position/LinearRangingPositionEstimator.java 249
com/irurueta/navigation/indoor/position/LinearRssiPositionEstimator.java 252
                            LinearMixedPositionEstimator.this);
                }
            }
        };
    }

    /**
     * Builds positions and distances for the internal lateration solver.
     */
    @SuppressWarnings("Duplicates")
    private void buildPositionsAndDistances() {
        if ((mUseHomogeneousLinearSolver && mHomogeneousTrilaterationSolver == null) ||
                (!mUseHomogeneousLinearSolver && mInhomogeneousTrilaterationSolver == null)) {
            return;
        }

        final int min = getMinRequiredSources();
        if (mSources == null || mFingerprint == null ||
                mSources.size() < min ||
                mFingerprint.getReadings() == null ||
                mFingerprint.getReadings().size() < min) {
            return;
        }

        final List<P> positions = new ArrayList<>();
        final List<Double> distances = new ArrayList<>();
        PositionEstimatorHelper.buildPositionsAndDistances(
                mSources, mFingerprint, positions, distances);

        setPositionsAndDistances(positions, distances);
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator2D.java 432
com/irurueta/navigation/indoor/radiosource/RangingAndRssiRadioSourceEstimator2D.java 420
        } else if (source instanceof Beacon) {
            final Beacon beacon = (Beacon) source;
            //transmitted power does not need to be estimated for beacons because
            //they broadcast such information
            return new BeaconWithPowerAndLocated2D(beacon.getIdentifiers(),
                    beacon.getTransmittedPower(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(), pathLossExponent,
                    transmittedPowerStandardDeviation,
                    pathLossExponentStandardDeviation, estimatedPosition,
                    estimatedPositionCovariance);
        } else {
            return null;
        }
    }

    /**
     * Creates inner estimators if needed.
     */
    @Override
    protected void createInnerEstimatorsIfNeeded() {
        if (mRangingInnerEstimator == null) {
            mRangingInnerEstimator = new RangingRadioSourceEstimator2D<>();
        }

        if (mRssiInnerEstimator == null &&
                (mTransmittedPowerEstimationEnabled || mPathLossEstimationEnabled)) {
            mRssiInnerEstimator = new RssiRadioSourceEstimator2D<>();
        }
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator3D.java 436
com/irurueta/navigation/indoor/radiosource/RangingAndRssiRadioSourceEstimator3D.java 422
        } else if (source instanceof Beacon) {
            final Beacon beacon = (Beacon) source;
            //transmitted power does not need to be estimated for beacons because
            //they broadcast such information
            return new BeaconWithPowerAndLocated3D(beacon.getIdentifiers(),
                    beacon.getTransmittedPower(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(), pathLossExponent,
                    transmittedPowerStandardDeviation,
                    pathLossExponentStandardDeviation, estimatedPosition,
                    estimatedPositionCovariance);
        } else {
            return null;
        }
    }

    /**
     * Creates inner estimators if needed.
     */
    @Override
    protected void createInnerEstimatorsIfNeeded() {
        if (mRangingInnerEstimator == null) {
            mRangingInnerEstimator = new RangingRadioSourceEstimator3D<>();
        }

        if (mRssiInnerEstimator == null &&
                (mTransmittedPowerEstimationEnabled || mPathLossEstimationEnabled)) {
            mRssiInnerEstimator = new RssiRadioSourceEstimator3D<>();
        }
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 1084
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 569
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 1096
            Solution<Point3D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than required minimum.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinReadings()) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/RangingRadioSourceEstimator2D.java 272
com/irurueta/navigation/indoor/radiosource/RangingRadioSourceEstimator3D.java 272
        Point2D[] positionsArray = new InhomogeneousPoint2D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];
        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);
        }

        if (mInitialPosition == null || !mNonLinearSolverEnabled) {
            if (mHomogeneousLinearSolver != null) {
                mHomogeneousLinearSolver.setPositionsAndDistances(positionsArray, distancesArray);
            }
            if (mInhomogeneousLinearSolver != null) {
                mInhomogeneousLinearSolver.setPositionsAndDistances(positionsArray, distancesArray);
            }
        }

        if (mNonLinearSolver != null && mNonLinearSolverEnabled) {
            mNonLinearSolver.setPositionsDistancesAndStandardDeviations(positionsArray,
                    distancesArray, distanceStandardDeviationsArray);
        }
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3657
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4054
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4328
                return initial;
            }

            @Override
            public void evaluate(final int i, final double[] point,
                                 final double[] result, final double[] params,
                                 final Matrix jacobian) {
                // We know that:
                //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                //  fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
                //  fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
                // sz, mxy, mxz, myx, myz, mzx and mzy is:

                // d(fmeasx)/d(bx) = 1.0
                // d(fmeasx)/d(by) = 0.0
                // d(fmeasx)/d(bz) = 0.0
                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myx) = 0.0
                // d(fmeasx)/d(myz) = 0.0
                // d(fmeasx)/d(mzx) = 0.0
                // d(fmeasx)/d(mzy) = 0.0

                // d(fmeasy)/d(bx) = 0.0
                // d(fmeasy)/d(by) = 1.0
                // d(fmeasy)/d(bz) = 0.0
                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myx) = ftruex
                // d(fmeasy)/d(myz) = ftruez
                // d(fmeasy)/d(mzx) = 0.0
                // d(fmeasy)/d(mzy) = 0.0

                // d(fmeasz)/d(bx) = 0.0
                // d(fmeasz)/d(by) = 0.0
                // d(fmeasz)/d(bz) = 1.0
                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myx) = 0.0
                // d(fmeasz)/d(myz) = 0.0
                // d(fmeasz)/d(mzx) = ftruex
                // d(fmeasz)/d(mzy) = ftruey

                final double bx = params[0];
                final double by = params[1];
                final double bz = params[2];

                final double sx = params[3];
                final double sy = params[4];
                final double sz = params[5];

                final double mxy = params[6];
                final double mxz = params[7];
                final double myx = params[8];
                final double myz = params[9];
                final double mzx = params[10];
                final double mzy = params[11];

                final double ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7050
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2837
                | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1802
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 469
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1821
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1702
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3965
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 476
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 4028
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 476
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2179
        final PROMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mStopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1823
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 493
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1843
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1722
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3988
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 499
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 4051
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 496
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2200
        final PROSACRobustEstimator<PreliminaryResult> innerEstimator =
                new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return mQualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3627
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3387
                        for (int j = 0; j < BodyKinematics.COMPONENTS; j++) {
                            for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {
                        mI = i;

                        // point contains fixed gravity versor values for current
                        // sequence
                        mPoint = point;

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateCommonAxis(i, params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1322
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1454
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3933
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 856
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 998
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4345
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double omegaMeasX = measuredKinematics.getAngularRateX();
            final double omegaMeasY = measuredKinematics.getAngularRateY();
            final double omegaMeasZ = measuredKinematics.getAngularRateZ();

            final double omegaTrueX = expectedKinematics.getAngularRateX();
            final double omegaTrueY = expectedKinematics.getAngularRateY();
            final double omegaTrueZ = expectedKinematics.getAngularRateZ();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 663
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 1889
        super(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(
                        new LMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 648
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1858
        super(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, bias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(
                        new LMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4302
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5424
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 6048
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 2854
                mNonLinearCalibrator.calibrate();

                result.mEstimatedMg = mNonLinearCalibrator.getEstimatedMg();
                result.mEstimatedGg = mNonLinearCalibrator.getEstimatedGg();
            }

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mMeasurements.size();

            final List<StandardDeviationFrameBodyKinematics> inlierMeasurements =
                    new ArrayList<>();
            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(mMeasurements.get(i));
                }
            }

            try {
                mNonLinearCalibrator.setInitialMg(preliminaryResult.mEstimatedMg);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4096
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5218
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/lateration/PROMedSRobustLateration2DSolver.java 608
com/irurueta/navigation/lateration/PROMedSRobustLateration3DSolver.java 610
            Point2D result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (mListener != null) {
                mListener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/lateration/PROSACRobustLateration2DSolver.java 635
com/irurueta/navigation/lateration/PROSACRobustLateration3DSolver.java 633
            Point2D result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (mListener != null) {
                mListener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4881
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8986
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5374
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8106
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6203
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7578
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6696
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9514
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5119
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9446
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5631
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8508
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6501
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7959
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7013
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9998
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias, initialMg,
                        initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias, initialMg,
                        initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias, initialMg,
                        initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10816
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10871
    public static void navigateECEF(final double timeInterval,
                                    final Distance oldX,
                                    final Distance oldY,
                                    final Distance oldZ,
                                    final CoordinateTransformation oldC,
                                    final ECEFVelocity oldVelocity,
                                    final Acceleration fx,
                                    final Acceleration fy,
                                    final Acceleration fz,
                                    final AngularSpeed angularRateX,
                                    final AngularSpeed angularRateY,
                                    final AngularSpeed angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, convertDistanceToDouble(oldX),
                convertDistanceToDouble(oldY), convertDistanceToDouble(oldZ), oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                convertAccelerationToDouble(fx),
                convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
                convertAngularSpeedToDouble(angularRateX),
                convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ),
                result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/lateration/RANSACRobustLateration2DSolver.java 207
com/irurueta/navigation/lateration/RANSACRobustLateration3DSolver.java 207
        super(circles, distanceStandardDeviations, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Solves the lateration problem.
     *
     * @return estimated position.
     * @throws LockedException          if instance is busy solving the lateration problem.
     * @throws NotReadyException        is solver is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public Point2D solve() throws LockedException, NotReadyException, RobustEstimatorException {
File Line
com/irurueta/navigation/lateration/RobustLateration2DSolver.java 1388
com/irurueta/navigation/lateration/RobustLateration3DSolver.java 1390
            Point2D estimatedPosition = mInitialPosition;
            if (mUseLinearSolver) {
                if (mUseHomogeneousLinearSolver) {
                    mHomogeneousLinearSolver.setPositionsAndDistances(mInnerPositions, mInnerDistances);
                    mHomogeneousLinearSolver.solve();
                    estimatedPosition = mHomogeneousLinearSolver.getEstimatedPosition();
                } else {
                    mInhomogeneousLinearSolver.setPositionsAndDistances(mInnerPositions, mInnerDistances);
                    mInhomogeneousLinearSolver.solve();
                    estimatedPosition = mInhomogeneousLinearSolver.getEstimatedPosition();
                }
            }

            if (mRefinePreliminarySolutions || estimatedPosition == null) {
                mNonLinearSolver.setInitialPosition(estimatedPosition);
                if (mDistanceStandardDeviations != null) {
                    mNonLinearSolver.setPositionsDistancesAndStandardDeviations(mInnerPositions,
                            mInnerDistances, mInnerDistanceStandardDeviations);
                } else {
                    mNonLinearSolver.setPositionsAndDistances(mInnerPositions, mInnerDistances);
                }
                mNonLinearSolver.solve();
                estimatedPosition = mNonLinearSolver.getEstimatedPosition();
            }

            solutions.add(estimatedPosition);
        } catch (final NavigationException ignore) {
            //if anything fails, no solution is added
        }
    }

    /**
     * Internally sets circles defining positions and euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than {@link #getMinRequiredPositionsAndDistances}.
     */
    private void internalSetCircles(final Circle[] circles) {
File Line
com/irurueta/navigation/gnss/GNSSEstimation.java 715
com/irurueta/navigation/gnss/GNSSMeasurement.java 485
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 1582
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 1756
    public void getPosition(final Point3D result) {
        result.setInhomogeneousCoordinates(mX, mY, mZ);
    }

    /**
     * Gets estimated ECEF user position expressed in meters (m).
     *
     * @return estimated ECEF user position.
     */
    public Point3D getPosition() {
        return new InhomogeneousPoint3D(mX, mY, mZ);
    }

    /**
     * Sets estimated ECEF user position expressed in meters (m).
     *
     * @param position estimated ECEF user position.
     */
    public void setPosition(final Point3D position) {
        mX = position.getInhomX();
        mY = position.getInhomY();
        mZ = position.getInhomZ();
    }

    /**
     * Gets estimatedECEF user position.
     *
     * @param result instance where result will be stored.
     */
    public void getEcefPosition(final ECEFPosition result) {
        result.setCoordinates(mX, mY, mZ);
    }

    /**
     * Gets estimated ECEF user position.
     *
     * @return estimated ECEF user position.
     */
    public ECEFPosition getEcefPosition() {
        return new ECEFPosition(mX, mY, mZ);
    }

    /**
     * Sets estimated ECEF user position.
     *
     * @param ecefPosition estimated ECEF user position.
     */
    public void setEcefPosition(final ECEFPosition ecefPosition) {
        mX = ecefPosition.getX();
        mY = ecefPosition.getY();
        mZ = ecefPosition.getZ();
    }

    /**
     * Gets estimated ECEF user velocity.
     *
     * @param result instance where result will be stored.
     */
    public void getEcefVelocity(final ECEFVelocity result) {
File Line
com/irurueta/navigation/gnss/GNSSEstimation.java 765
com/irurueta/navigation/gnss/GNSSMeasurement.java 704
        mZ = ecefPosition.getZ();
    }

    /**
     * Gets estimated ECEF user velocity.
     *
     * @param result instance where result will be stored.
     */
    public void getEcefVelocity(final ECEFVelocity result) {
        result.setCoordinates(mVx, mVy, mVz);
    }

    /**
     * Gets estimated ECEF user velocity.
     *
     * @return estimated ECEF user velocity.
     */
    public ECEFVelocity getEcefVelocity() {
        return new ECEFVelocity(mVx, mVy, mVz);
    }

    /**
     * Sets estimated ECEF user velocity.
     *
     * @param ecefVelocity estimated ECEF user velocity.
     */
    public void setEcefVelocity(final ECEFVelocity ecefVelocity) {
        mVx = ecefVelocity.getVx();
        mVy = ecefVelocity.getVy();
        mVz = ecefVelocity.getVz();
    }

    /**
     * Gets estimated ECEF user position and velocity.
     *
     * @param result instance where result will be stored.
     */
    public void getPositionAndVelocity(final ECEFPositionAndVelocity result) {
        result.setPositionCoordinates(mX, mY, mZ);
        result.setVelocityCoordinates(mVx, mVy, mVz);
    }

    /**
     * Gets estimated ECEF user position and velocity.
     *
     * @return estimated ECEF user position and velocity.
     */
    public ECEFPositionAndVelocity getPositionAndVelocity() {
        return new ECEFPositionAndVelocity(mX, mY, mZ, mVx, mVy, mVz);
    }

    /**
     * Sets estimated ECEF user position and velocity.
     *
     * @param positionAndVelocity estimated ECEF user position and velocity.
     */
    public void setPositionAndVelocity(
            final ECEFPositionAndVelocity positionAndVelocity) {
File Line
com/irurueta/navigation/indoor/RadioSourceKNearestFinder.java 198
com/irurueta/navigation/indoor/RadioSourceNoMeanKNearestFinder.java 202
            final double sqrDist = f.sqrDistanceTo(fingerprint);
            if (sqrDist < maxSqrDist || nearestSqrDistances.size() < k) {

                //find insertion point
                int pos = -1;
                int i = 0;
                for (final Double sd : nearestSqrDistances) {
                    if (sqrDist < sd) {
                        //insertion point found
                        pos = i;
                        break;
                    }
                    i++;
                }

                if (pos >= 0) {
                    nearestSqrDistances.add(pos, sqrDist);
                    nearestFingerprints.add(pos, f);
                } else {
                    nearestSqrDistances.add(sqrDist);
                    nearestFingerprints.add(f);
                }

                //remove results exceeding required number of k neighbours to be found
                if (nearestFingerprints.size() > k) {
                    nearestSqrDistances.remove(k);
                    nearestFingerprints.remove(k);
                }

                //update maxSqrDist to the largest squared distance value contained in result list distances
                maxSqrDist = nearestSqrDistances.get(nearestSqrDistances.size() - 1);
            }
        }
    }
}
File Line
com/irurueta/navigation/inertial/calibration/BodyKinematicsGenerator.java 117
com/irurueta/navigation/inertial/calibration/BodyKinematicsGenerator.java 328
                                final Collection<BodyKinematics> result) {
        try {
            final Matrix trueFibb = new Matrix(BodyKinematics.COMPONENTS, 1);
            final Matrix ma = errors.getAccelerometerScaleFactorAndCrossCouplingErrors();
            final Matrix ba = errors.getAccelerometerBiasesAsMatrix();
            final Matrix trueOmegaIbb = new Matrix(BodyKinematics.COMPONENTS, 1);
            final Matrix mg = errors.getGyroScaleFactorAndCrossCouplingErrors();
            final Matrix bg = errors.getGyroBiasesAsMatrix();
            final Matrix gg = errors.getGyroGDependentBiases();
            final Matrix identity = Matrix.identity(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            final Matrix tmp33 = new Matrix(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            final Matrix tmp31a = new Matrix(BodyKinematics.COMPONENTS, 1);
            final Matrix tmp31b = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4059
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4406
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, biasX, biasY, biasZ, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores,
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3269
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4513
                        for (int j = 0; j < BodyKinematics.COMPONENTS; j++) {
                            for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        // g-dependent cross biases G
                        final int num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                        for (int i = 0, j = k; i < num; i++, j++) {
                            initial[j] = initialG.getElementAtIndex(i);
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3836
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3566
        setResult(m, b);
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter.
     *
     * @throws AlgebraException if there are numerical instabilities.
     */
    private void setInputData() throws AlgebraException {

        final Matrix ba = getAccelerometerBiasAsMatrix();
        final Matrix ma = getAccelerometerMa();

        final double[] measuredBeforeF = new double[BodyKinematics.COMPONENTS];
        final double[] fixedBeforeF = new double[BodyKinematics.COMPONENTS];

        final double[] measuredAfterF = new double[BodyKinematics.COMPONENTS];
        final double[] fixedAfterF = new double[BodyKinematics.COMPONENTS];

        final int numSequences = mSequences.size();
        final Matrix x = new Matrix(numSequences,
                2 * BodyKinematics.COMPONENTS);
        final double[] y = new double[numSequences];
        final double[] standardDeviations = new double[numSequences];

        // make a copy of input sequences that will be used to update
        // kinematics measurements with fixed values for memory efficiency

        mFixedSequences = new ArrayList<>();
        for (final BodyKinematicsSequence<StandardDeviationTimedBodyKinematics> sequence : mSequences) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3055
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4754
                        for (int j = 0; j < BodyKinematics.COMPONENTS; j++) {
                            for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        // g-dependent cross biases G
                        final int num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                        for (int i = 0, j = k; i < num; i++, j++) {
                            initial[j] = initialG.getElementAtIndex(i);
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3794
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5374
            final int i, final double[] params)
            throws EvaluationException {

        final double m11 = params[0];

        final double m12 = params[1];
        final double m22 = params[2];

        final double m13 = params[3];
        final double m23 = params[4];
        final double m33 = params[5];

        final double g11 = params[6];
        final double g21 = params[7];
        final double g31 = params[8];

        final double g12 = params[9];
        final double g22 = params[10];
        final double g32 = params[11];

        final double g13 = params[12];
        final double g23 = params[13];
        final double g33 = params[14];

        return evaluate(i, m11, 0.0, 0.0, m12, m22, 0.0,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 76
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 76
public abstract class RobustEasyGyroscopeCalibrator {

    /**
     * Indicates whether by default a common z-axis is assumed for both the accelerometer
     * and gyroscope.
     */
    public static final boolean DEFAULT_USE_COMMON_Z_AXIS = true;

    /**
     * Indicates that by default G-dependent cross biases introduced
     * by the accelerometer on the gyroscope are estimated.
     */
    public static final boolean DEFAULT_ESTIMATE_G_DEPENDENT_CROSS_BIASES = true;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD =
            RobustEstimatorMethod.LMedS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen subsamples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Known x-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasX;

    /**
     * Known y-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasY;

    /**
     * Known z-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasZ;

    /**
     * Known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSx;

    /**
     * Known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSy;

    /**
     * Known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSz;

    /**
     * Known accelerometer x-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMxy;

    /**
     * Know accelerometer x-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMxz;

    /**
     * Known accelerometer y-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMyx;

    /**
     * Known accelerometer y-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMyz;

    /**
     * Known accelerometer z-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMzx;

    /**
     * Known accelerometer z-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMzy;

    /**
     * Initial x-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     */
    private double mInitialBiasX;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4133
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4485
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX,
                        biasY, biasZ, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(biasX,
                        biasY, biasZ, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX,
                        biasY, biasZ, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, biasX, biasY, biasZ, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 284
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1060
            setupWmmEstimator();

            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 299
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1059
            setupWmmEstimator();

            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator2D.java 1054
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator3D.java 1052
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator2D.java 548
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator3D.java 547
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator2D.java 1065
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator3D.java 1066
            final Solution<Point2D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinReadings()) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 1079
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 1084
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 570
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 569
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 1099
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 1096
            final Solution<Point2D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than required minimum.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < getMinReadings()) {
            throw new IllegalArgumentException();
        }

        mQualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7515
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3757
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5313
                        return BodyKinematics.COMPONENTS;
                    }

                    @Override
                    public double[] createInitialParametersArray() {
                        final double[] initial = new double[GENERAL_UNKNOWNS];

                        // biases b
                        for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                            initial[i] = initialB.getElementAtIndex(i);
                        }

                        // cross coupling errors M
                        final int num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                        for (int i = 0, j = BodyKinematics.COMPONENTS; i < num; i++, j++) {
                            initial[j] = initialM.getElementAtIndex(i);
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(final int i, final double[] point,
                                           final double[] params, final double[] derivatives)
                            throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 169
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 641
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 713
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 626
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 176
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 157
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 934
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<PreliminaryResult> innerEstimator =
                new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1351
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3846
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1319
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3909
                estimateGDependentCrossBiases, bias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1347
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3842
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @param qualityScores quality scores corresponding to each sequence.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1315
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3905
                estimateGDependentCrossBiases, bias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @param qualityScores quality scores corresponding to each sequence.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3605
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3721
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must have length 3 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must
     *                          have length 3 and is expressed in
     *                          meters per squared second
     *                          (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param listener          listener to handle events raised by this
     *                          calibrator.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3399
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3515
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must have length 3 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must
     *                          have length 3 and is expressed in
     *                          meters per squared second
     *                          (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param listener          listener to handle events raised by this
     *                          calibrator.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5041
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7752
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5205
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7925
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5374
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9514
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6363
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9157
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6527
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9333
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6696
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8106
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5282
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8136
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5449
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8316
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5631
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9998
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6664
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9623
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6831
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9804
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7013
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8508
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias, initialMg,
                        initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias, initialMg,
                        initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias, initialMg,
                        initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/indoor/Utils.java 507
com/irurueta/navigation/indoor/Utils.java 1207
com/irurueta/navigation/indoor/Utils.java 2165
com/irurueta/navigation/indoor/Utils.java 2580
            final Double fingerprintRssiVariance,
            final Double pathLossExponentVariance,
            final Matrix fingerprintPositionCovariance,
            final Matrix radioSourcePositionCovariance,
            final Matrix estimatedPositionCovariance) throws IndoorException {

        if (fingerprintPosition == null || radioSourcePosition == null ||
                estimatedPosition == null) {
            return null;
        }

        //1st order Taylor expression of received power in 3D:
        //Pr(pi) = Pr(p1)
        //  - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
        //  - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
        //  - 10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1)
        //where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2

        final double x1 = fingerprintPosition.getInhomX();
        final double y1 = fingerprintPosition.getInhomY();
        final double z1 = fingerprintPosition.getInhomZ();

        final double xa = radioSourcePosition.getInhomX();
        final double ya = radioSourcePosition.getInhomY();
        final double za = radioSourcePosition.getInhomZ();

        final double xi = estimatedPosition.getInhomX();
        final double yi = estimatedPosition.getInhomY();
        final double zi = estimatedPosition.getInhomZ();

        final double[] mean = new double[]{
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3758
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3796
                final double mzy = params[8];

                final double g11 = params[9];
                final double g21 = params[10];
                final double g31 = params[11];
                final double g12 = params[12];
                final double g22 = params[13];
                final double g32 = params[14];
                final double g13 = params[15];
                final double g23 = params[16];
                final double g33 = params[17];

                final double omegatruex = point[0];
                final double omegatruey = point[1];
                final double omegatruez = point[2];

                final double ftruex = point[3];
                final double ftruey = point[4];
                final double ftruez = point[5];

                result[0] = mBiasX + omegatruex + sx * omegatruex + mxy * omegatruey + mxz * omegatruez
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 566
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2270
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 562
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2269
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 589
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2295
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 590
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2293
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5041
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9157
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5205
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9333
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6363
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7752
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6527
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7925
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 2756
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 12505
                        previousEcefFrame);

        final double angularRateMeasX1 = measuredKinematics.getAngularRateX();
        final double angularRateMeasY1 = measuredKinematics.getAngularRateY();
        final double angularRateMeasZ1 = measuredKinematics.getAngularRateZ();

        final double angularRateTrueX = expectedKinematics.getAngularRateX();
        final double angularRateTrueY = expectedKinematics.getAngularRateY();
        final double angularRateTrueZ = expectedKinematics.getAngularRateZ();

        final double fTrueX = expectedKinematics.getFx();
        final double fTrueY = expectedKinematics.getFy();
        final double fTrueZ = expectedKinematics.getFz();

        final double[] b = preliminaryResult.mEstimatedBiases;
        final double bx = b[0];
        final double by = b[1];
        final double bz = b[2];

        final Matrix mg = preliminaryResult.mEstimatedMg;

        final Matrix gg = preliminaryResult.mEstimatedGg;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5282
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9623
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5449
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9804
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6664
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8136
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6831
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8316
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/FrameBodyKinematics.java 350
com/irurueta/navigation/inertial/calibration/FrameBodyMagneticFluxDensity.java 376
    }

    /**
     * Gets current body position (which will typically remain constant),
     * velocity (which will typically be zero) and orientation (which usually changes
     * with each measurement to perform calibration of a single device) resolved
     * around ECEF axes associated to body kinematics measurement.
     *
     * @return current ECEF frame associated to body kinematics measurement or null if
     * not available.
     */
    public ECEFFrame getFrame() {
        return mFrame;
    }

    /**
     * Sets current body position (which will typically remain constant),
     * velocity (which will typically be zero) and orientation (which usually changes
     * with each measurement to perform calibration of a single device) resolved
     * around ECEF axes associated to body kinematics measurement.
     *
     * @param frame current ECEF frame
     */
    public void setFrame(final ECEFFrame frame) {
        mFrame = frame;
    }

    /**
     * Gets current body position (which will typically remain constant),
     * velocity (which will typically be zero) and orientation (which usually changes
     * with each measurement to perform calibration of a single device) resolved
     * around NED axes associated to body kinematics measurement.
     *
     * @return current NED frame associated to body kinematics measurement or null if
     * not available.
     */
    public NEDFrame getNedFrame() {
        return mFrame != null ?
                ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(mFrame) : null;
    }

    /**
     * Gets current body position (which will typically remain constant),
     * velocity (which will typically be zero) and orientation (which usually changes
     * with each measurement to perform calibration of a single device) resolved
     * around NED axes associated to body kinematics measurement.
     *
     * @param result instance where result data will be stored if available.
     * @return true if result instance was updated, false otherwise.
     */
    public boolean getNedFrame(final NEDFrame result) {
        if (mFrame != null) {
            ECEFtoNEDFrameConverter.convertECEFtoNED(mFrame, result);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets current body position (which will typically remain constant),
     * velocity (which will typically be zero) and orientation (which usually changes
     * with each measurement to perform calibration of a single device) resolved
     * around NED axes associated to body kinematics measurement.
     * <p>
     * This method will internally store the corresponding ECEF frame to provided
     * NED frame value.
     *
     * @param nedFrame current NED frame associated to body kinematics measurement
     *                 to be set.
     */
    public void setNedFrame(final NEDFrame nedFrame) {
        if (nedFrame != null) {
            if (mFrame != null) {
                NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, mFrame);
            } else {
                mFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
            }
        } else {
            mFrame = null;
        }
    }

    /**
     * Gets previous body position (which will typically remain constant),
     * velocity (which will typically be zero) and orientation (which usually changes
     * with each measurement to perform calibration of a single device) resolved
     * around ECEF axes associated to previous body kinematics measurement.
     *
     * @return previous ECEF frame associated to previous body kinematics measurement or
     * null if not available.
     */
    public ECEFFrame getPreviousFrame() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7903
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5539
            mFmeas.setElementAtIndex(0, mFmeasX);
            mFmeas.setElementAtIndex(1, mFmeasY);
            mFmeas.setElementAtIndex(2, mFmeasZ);

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);

            mB.setElementAtIndex(0, bx);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1778
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1887
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1906
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 4051
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 4114
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1805
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1909
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1933
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 4075
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 4140
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null ||
                qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4108
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4452
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ, commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ, commonAxisUsed);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores,
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2880
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3541
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3060
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3731
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 664
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1859
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(
                        new LMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 649
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 1890
                estimateGDependentCrossBiases, bias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(
                        new LMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1821
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1841
    public MSACRobustKnownBiasTurntableGyroscopeCalibrator(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener) {
        super(position, turntableRotationRate, timeInterval, measurements,
                commonAxisUsed, estimateGDependentCrossBiases, bias,
                initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1851
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1871
    public MSACRobustTurntableGyroscopeCalibrator(
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener) {
        super(position, turntableRotationRate, timeInterval, measurements,
                commonAxisUsed, estimateGDependentCrossBiases, initialBias,
                initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1554
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1522
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1576
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1546
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 847
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 832
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4079
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5176
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4182
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4532
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ,
                        commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ,
                        commonAxisUsed);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3873
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4970
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 284
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1060
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 562
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2265
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 585
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2289
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 376
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1139
            setupWmmEstimator();

            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 299
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1059
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 558
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2266
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 586
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2291
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 376
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1137
            setupWmmEstimator();

            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3092
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3831
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3201
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3940
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/indoor/Utils.java 753
com/irurueta/navigation/indoor/Utils.java 1919
                    jacobian.setElementAtIndex(0, derivativeFingerprintRssi);
                    jacobian.setElementAtIndex(1, derivativePathLossExponent);
                    jacobian.setElementAtIndex(2, derivativeX1);
                    jacobian.setElementAtIndex(3, derivativeY1);
                    jacobian.setElementAtIndex(4, derivativeZ1);
                    jacobian.setElementAtIndex(5, derivativeXa);
                    jacobian.setElementAtIndex(6, derivativeYa);
                    jacobian.setElementAtIndex(7, derivativeZa);
                    jacobian.setElementAtIndex(8, derivativeXi);
                    jacobian.setElementAtIndex(9, derivativeYi);
                    jacobian.setElementAtIndex(10, derivativeZi);
                }

                @Override
                public int getNumberOfVariables() {
                    return 1;
                }
            }, mean, covariance);
        } catch (final AlgebraException | StatisticsException e) {
            throw new IndoorException(e);
        }
    }

    /**
     * Propagates provided variances (fingerprint rssi variance, path-loss exponent variance,
     * fingerprint position covariance and radio source position covariance) into
     * rssi variance by considering the 2D 2nd order Taylor expression of received power.
     * Notice that any unknown variance is assumed to be zero.
     *
     * @param fingerprintRssi               closest located fingerprint reading RSSI expressed in dBm's.
     * @param pathLossExponent              path-loss exponent.
     * @param fingerprintPosition           position of closest fingerprint.
     * @param radioSourcePosition           radio source position associated to fingerprint reading.
     * @param estimatedPosition             position to be estimated. Usually this is equal to the
     *                                      initial position used by a non linear algorithm.
     * @param fingerprintRssiVariance       variance of fingerprint RSSI or null if unknown.
     * @param pathLossExponentVariance      variance of path-loss exponent or null if unknown.
     * @param fingerprintPositionCovariance covariance of fingerprint position or null if
     *                                      unknown.
     * @param radioSourcePositionCovariance covariance of radio source position or null
     *                                      if unknown.
     * @param estimatedPositionCovariance   covariance of position to be estimated or null
     *                                      if unknown. (This is usually unknown).
     * @return a normal distribution containing expected received RSSI value and its variance.
     * @throws IndoorException if something fails.
     */
    public static MultivariateNormalDist propagateVariancesToRssiVarianceSecondOrderNonLinear2D(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3521
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4189
                jacobian.setElementAt(2, 5, ftruez);
                jacobian.setElementAt(2, 6, 0.0);
                jacobian.setElementAt(2, 7, 0.0);
                jacobian.setElementAt(2, 8, 0.0);
            }
        });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double sx = result[3];
        final double sy = result[4];
        final double sz = result[5];

        final double mxy = result[6];
        final double mxz = result[7];
        final double myz = result[8];

        if (mEstimatedBiases == null) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7905
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5993
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2906
            mFmeas.setElementAtIndex(2, mFmeasZ);

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);

            mB.setElementAtIndex(0, bx);
            mB.setElementAtIndex(1, by);
            mB.setElementAtIndex(2, bz);

            mInvM.multiply(mFmeas, mFtrue);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3791
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2536
                        return evaluateGeneral(i, params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];
        final double m21 = result[4];
        final double m31 = result[5];

        final double m12 = result[6];
        final double m22 = result[7];
        final double m32 = result[8];

        final double m13 = result[9];
        final double m23 = result[10];
        final double m33 = result[11];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 769
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 754
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1555
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1523
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1577
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1547
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 848
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 833
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
File Line
com/irurueta/navigation/indoor/position/LinearMixedPositionEstimator3D.java 186
com/irurueta/navigation/indoor/position/LinearRangingAndRssiPositionEstimator3D.java 188
com/irurueta/navigation/indoor/position/LinearRangingPositionEstimator3D.java 188
com/irurueta/navigation/indoor/position/LinearRssiPositionEstimator3D.java 187
            final List<Point3D> positions, final List<Double> distances) {
        final int size = positions.size();
        Point3D[] positionsArray = new InhomogeneousPoint3D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
        }

        try {
            mHomogeneousTrilaterationSolver.setPositionsAndDistances(positionsArray,
                    distancesArray);
            mInhomogeneousTrilaterationSolver.setPositionsAndDistances(positionsArray,
                    distancesArray);
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }

    /**
     * Initializes lateration solver.
     */
    private void init() {
        mHomogeneousTrilaterationSolver = new HomogeneousLinearLeastSquaresLateration3DSolver(
                mLaterationSolverListener);
        mInhomogeneousTrilaterationSolver = new InhomogeneousLinearLeastSquaresLateration3DSolver(
                mLaterationSolverListener);
    }
}
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanInitializer.java 64
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanInitializer.java 68
        result.initialize(0.0);

        for (int i = 0; i < 3; i++) {
            result.setElementAt(i, i, initAttUnc2);
        }
        for (int i = 3; i < 6; i++) {
            result.setElementAt(i, i, initVelUnc2);
        }
        for (int i = 6; i < 9; i++) {
            result.setElementAt(i, i, initPosUnc2);
        }
        for (int i = 9; i < 12; i++) {
            result.setElementAt(i, i, initBaUnc2);
        }
        for (int i = 12; i < 15; i++) {
            result.setElementAt(i, i, initBgUnc2);
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7050
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4297
                | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1774
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1801
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 905
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1883
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 550
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1902
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1905
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 578
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1929
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 993
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 372
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 1004
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1785
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 4047
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 558
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 4110
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1808
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 4071
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 585
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 4136
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 916
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 2040
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 379
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 2070
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2882
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3037
            final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3035
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3697
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param initialMa    initial scale factors and cross coupling errors matrix.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3115
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3777
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final Matrix initialMa,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3157
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3819
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias,
            final Matrix initialMa, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3543
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3699
            final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4135
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4320
            final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4884
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5070
            final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3062
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3218
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3216
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3887
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param initialMa    initial scale factors and cross coupling errors matrix.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3296
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3967
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMa,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa,
                        listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3343
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4014
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias,
            final Matrix initialMa, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3733
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3889
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4337
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4531
            final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5107
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5302
            final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3094
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3271
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3269
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4008
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param initialMm    initial soft-iron matrix containing scale factors
     *                     and cross coupling errors.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3364
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4103
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron, final Matrix initialMm,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param initialMm      initial soft-iron matrix containing scale factors
     *                       and cross coupling errors.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3411
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4150
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron,
            final Matrix initialMm,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param initialMm      initial soft-iron matrix containing scale factors
     *                       and cross coupling errors.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3833
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4010
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4757
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4957
            final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5604
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5807
            final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements,
                        hardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements,
                        hardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3203
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3380
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3378
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4117
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3473
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4212
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron, final Matrix initialMm,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3520
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4259
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final Matrix initialMm,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3942
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4119
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4866
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5066
            final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5713
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5916
            final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements,
                        initialHardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements,
                        initialHardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingRadioSourceEstimator2D.java 722
com/irurueta/navigation/indoor/radiosource/RobustRangingRadioSourceEstimator3D.java 725
            return new BeaconLocated2D(beacon.getIdentifiers(),
                    beacon.getTransmittedPower(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(), estimatedPosition,
                    estimatedPositionCovariance);
        } else {
            return null;
        }
    }

    /**
     * Indicates whether an homogeneous linear solver is used to estimate an initial
     * position.
     *
     * @return true if homogeneous linear solver is used, false if an inhomogeneous linear
     * one is used instead.
     */
    @Override
    public boolean isHomogeneousLinearSolverUsed() {
        return mInnerEstimator.isHomogeneousLinearSolverUsed();
    }

    /**
     * Specifies whether an homogeneous linear solver is used to estimate an initial
     * position.
     *
     * @param useHomogeneousLinearSolver true if homogeneous linear solver is used, false
     *                                   if an inhomogeneous linear one is used instead.
     * @throws LockedException if estimator is locked.
     */
    @Override
    public void setHomogeneousLinearSolverUsed(
            final boolean useHomogeneousLinearSolver) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInnerEstimator.setHomogeneousLinearSolverUsed(useHomogeneousLinearSolver);
    }

    /**
     * Solves preliminar solution for a subset of samples.
     *
     * @param samplesIndices indices of subset samples.
     * @param solutions      instance where solution will be stored.
     */
    @Override
    protected void solvePreliminarSolutions(
            final int[] samplesIndices,
            final List<Solution<Point2D>> solutions) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1028
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2667
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2934
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1035
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3193
    }

    /**
     * Indicates whether estimator is ready to start the estimator.
     *
     * @return true if estimator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null
                && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is currently running or not.
     *
     * @return true if estimator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if estimator is currently running.
     * @throws NotReadyException    if estimator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 370
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3046
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 384
    }

    /**
     * Indicates whether calibrator is ready to start the calibration.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return mMeasurements != null
                && mMeasurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2121
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2326
        this(measurements, commonAxisUsed, initialBias, initialMa);
        mListener = listener;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7571
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3504
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3815
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5025
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5370
        final double m33 = result[11];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
        b.setElementAtIndex(0, bx);
        b.setElementAtIndex(1, by);
        b.setElementAtIndex(2, bz);

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, m21);
        m.setElementAtIndex(2, m31);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, m32);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7731
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3333
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3673
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4821
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5208
        final double m33 = result[8];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
        b.setElementAtIndex(0, bx);
        b.setElementAtIndex(1, by);
        b.setElementAtIndex(2, bz);

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, 0.0);
        m.setElementAtIndex(2, 0.0);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 829
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1775
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1802
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 906
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 913
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 292
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 923
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1884
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 551
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1903
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1906
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 579
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1930
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 994
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 373
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 1005
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 840
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1960
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 300
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1992
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1786
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 4048
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 559
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 4111
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1809
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 4072
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 586
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 4137
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 917
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 2041
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 380
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 2071
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2793
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3084
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param biasX  known x coordinate of accelerometer bias.
     * @param biasY  known y coordinate of accelerometer bias.
     * @param biasZ  known z coordinate of accelerometer bias.
     * @param method robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 2774
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 2856
                result.mEstimatedMa = mNonLinearCalibrator.getEstimatedMa();
            }

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mMeasurements.size();

            final List<StandardDeviationFrameBodyKinematics> inlierMeasurements =
                    new ArrayList<>();
            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(mMeasurements.get(i));
                }
            }

            try {
                mNonLinearCalibrator.setInitialBias(preliminaryResult.mEstimatedBiases);
                mNonLinearCalibrator.setInitialMa(preliminaryResult.mEstimatedMa);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2780
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2738
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4176
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4240
                && mSequences.size() >= getMinimumRequiredSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                if (mEstimateGDependentCrossBiases) {
                    calibrateCommonAxisAndGDependentCrossBiases();
                } else {
                    calibrateCommonAxis();
                }
            } else {
                if (mEstimateGDependentCrossBiases) {
                    calibrateGeneralAndGDependentCrossBiases();
                } else {
                    calibrateGeneral();
                }
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final FittingException
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3539
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3303
        setResult(m, b, g);
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and G-dependent cross biases
     * are ignored.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxis()
            throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
        // The gyroscope model is
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Where Mg is upper triangular

        // Since G-dependent cross biases are ignored, we can assume that Gg = 0

        // Hence:
        // Ωmeas = bg + (I + Mg) * Ωtrue

        // For convergence purposes of the Levenberg-Marquardt algorithm,
        // the gyroscope model can be better expressed as:
        // Ωmeas = T*K*(Ωtrue + b)
        // Ωmeas = M*(Ωtrue + b)
        // Ωmeas = M*Ωtrue + M*b

        // Hence
        // Ωmeas - M*b = M*Ωtrue

        // M^-1 * (Ωmeas - M*b) = Ωtrue

        // where:
        // M = I + Mg
        // bg = M*b = (I + Mg)*b --> b = M^-1*bg

        // Notice that M is upper diagonal because Mg is upper diagonal
        // when common axis is assumed

        final GradientEstimator gradientEstimator = new GradientEstimator(
                new MultiDimensionFunctionEvaluatorListener() {
                    @Override
                    public double evaluate(final double[] params)
                            throws EvaluationException {
                        return evaluateCommonAxis(mI, params);
                    }
                });

        final Matrix initialM = Matrix.identity(
                BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        initialM.add(getInitialMg());

        // Force initial M to be upper diagonal
        initialM.setElementAt(1, 0, 0.0);
        initialM.setElementAt(2, 0, 0.0);
        initialM.setElementAt(2, 1, 0.0);

        final Matrix invInitialM = Utils.inverse(initialM);
        final Matrix initialBg = getInitialBiasAsMatrix();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4399
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4509
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      sequence. The larger the score value the better
     *                      the quality of the sequence.
     * @param sequences     collection of sequences containing timestamped body
     *                      kinematics measurements.
     * @param initialBias   initial gyroscope bias to be used to find a solution.
     *                      This must be 3x1 and is expressed in radians per
     *                      second (rad/s).
     * @param initialMg     initial gyroscope scale factors and cross coupling
     *                      errors matrix. Must be 3x3.
     * @param initialGg     initial gyroscope G-dependent cross biases
     *                      introduced on the gyroscope by the specific forces
     *                      sensed by the accelerometer. Must be 3x3.
     * @param listener      listener to handle events raised by this
     *                      calibrator.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2864
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3160
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param biasX  known x coordinate of gyroscope bias.
     * @param biasY  known y coordinate of gyroscope bias.
     * @param biasZ  known z coordinate of gyroscope bias.
     * @param method robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final AngularSpeed biasX, final AngularSpeed biasY,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4193
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4303
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias, initialMg,
                        initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        qualityScores, sequences, initialBias, initialMg,
                        initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      sequence. The larger the score value the better
     *                      the quality of the sequence.
     * @param sequences     collection of sequences containing timestamped body
     *                      kinematics measurements.
     * @param initialBias   initial gyroscope bias to be used to find a solution.
     *                      This must be 3x1 and is expressed in radians per
     *                      second (rad/s).
     * @param initialMg     initial gyroscope scale factors and cross coupling
     *                      errors matrix. Must be 3x3.
     * @param initialGg     initial gyroscope G-dependent cross biases
     *                      introduced on the gyroscope by the specific forces
     *                      sensed by the accelerometer. Must be 3x3.
     * @param listener      listener to handle events raised by this
     *                      calibrator.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 1860
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 696
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2707
        mListener = listener;
    }

    /**
     * Gets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return known x coordinate of accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return mBiasX;
    }

    /**
     * Sets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasX known x coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
    }

    /**
     * Gets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return known y coordinate of accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return mBiasY;
    }

    /**
     * Sets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasY known y coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = biasY;
    }

    /**
     * Gets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return known z coordinate of accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return mBiasZ;
    }

    /**
     * Sets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasZ known z coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = biasZ;
    }

    /**
     * Gets known x coordinate of accelerometer bias.
     *
     * @return known x coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2539
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6369
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2806
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6809
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1665
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1774
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1144
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1816
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS ||
                initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMa.getElementAtIndex(0);
        mInitialMyx = initialMa.getElementAtIndex(1);
        mInitialMzx = initialMa.getElementAtIndex(2);

        mInitialMxy = initialMa.getElementAtIndex(3);
        mInitialSy = initialMa.getElementAtIndex(4);
        mInitialMzy = initialMa.getElementAtIndex(5);

        mInitialMxz = initialMa.getElementAtIndex(6);
        mInitialMyz = initialMa.getElementAtIndex(7);
        mInitialSz = initialMa.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6264
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1272
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1666
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3648
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1300
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3606
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @return known bias as a column matrix.
     */
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mBiasX);
        result.setElementAtIndex(1, mBiasY);
        result.setElementAtIndex(2, mBiasZ);
    }

    /**
     * Sets known bias as an array.
     *
     * @param bias bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setBias(final Matrix bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4133
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4882
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4181
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4930
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 6260
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 6526
            mInnerCalibrator.calibrate();

            result.mEstimatedMa = mInnerCalibrator.getEstimatedMa();

            solutions.add(result);

        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mMeasurements.size();

            final List<StandardDeviationBodyKinematics> inlierMeasurements =
                    new ArrayList<>();
            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(mMeasurements.get(i));
                }
            }

            try {
                mInnerCalibrator.setBias(mBiasX, mBiasY, mBiasZ);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4335
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5105
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4386
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5156
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2566
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2530
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2524
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3756
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3013
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2568
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1690
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2530
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3714
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1155
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3774
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3819
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS ||
                initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMg.getElementAtIndex(0);
        mInitialMyx = initialMg.getElementAtIndex(1);
        mInitialMzx = initialMg.getElementAtIndex(2);

        mInitialMxy = initialMg.getElementAtIndex(3);
        mInitialSy = initialMg.getElementAtIndex(4);
        mInitialMzy = initialMg.getElementAtIndex(5);

        mInitialMxz = initialMg.getElementAtIndex(6);
        mInitialMyz = initialMg.getElementAtIndex(7);
        mInitialSz = initialMg.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3468
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3791
                        return evaluateGeneralWithGDependentCrossBiases(i, params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];
        final double m21 = result[4];
        final double m31 = result[5];

        final double m12 = result[6];
        final double m22 = result[7];
        final double m32 = result[8];

        final double m13 = result[9];
        final double m23 = result[10];
        final double m33 = result[11];

        final double g11 = result[12];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4224
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5343
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4018
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5137
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3525
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3483
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1758
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 1774
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1041
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1016
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1829
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1845
    public void setInitialMm(final Matrix initialMm)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS ||
                initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        mInitialSx = initialMm.getElementAtIndex(0);
        mInitialMyx = initialMm.getElementAtIndex(1);
        mInitialMzx = initialMm.getElementAtIndex(2);

        mInitialMxy = initialMm.getElementAtIndex(3);
        mInitialSy = initialMm.getElementAtIndex(4);
        mInitialMzy = initialMm.getElementAtIndex(5);

        mInitialMxz = initialMm.getElementAtIndex(6);
        mInitialMyz = initialMm.getElementAtIndex(7);
        mInitialSz = initialMm.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4755
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5602
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4807
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5654
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4864
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5711
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4916
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5763
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10710
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10763
    public static void navigateECEF(final double timeInterval,
                                    final ECEFPosition oldPosition,
                                    final CoordinateTransformation oldC,
                                    final Speed oldSpeedX,
                                    final Speed oldSpeedY,
                                    final Speed oldSpeedZ,
                                    final Acceleration fx,
                                    final Acceleration fy,
                                    final Acceleration fz,
                                    final AngularSpeed angularRateX,
                                    final AngularSpeed angularRateY,
                                    final AngularSpeed angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldSpeedX, oldSpeedY, oldSpeedZ, convertAccelerationToDouble(fx),
                convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
                convertAngularSpeedToDouble(angularRateX),
                convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ),
                result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11135
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11188
    public static void navigateECEF(final double timeInterval,
                                    final ECEFPosition oldPosition,
                                    final CoordinateTransformation oldC,
                                    final double oldVx,
                                    final double oldVy,
                                    final double oldVz,
                                    final Acceleration fx,
                                    final Acceleration fy,
                                    final Acceleration fz,
                                    final AngularSpeed angularRateX,
                                    final AngularSpeed angularRateY,
                                    final AngularSpeed angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz,
                convertAccelerationToDouble(fx), convertAccelerationToDouble(fy),
                convertAccelerationToDouble(fz),
                convertAngularSpeedToDouble(angularRateX),
                convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11241
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11294
    public static void navigateECEF(final double timeInterval,
                                    final double oldX,
                                    final double oldY,
                                    final double oldZ,
                                    final CoordinateTransformation oldC,
                                    final ECEFVelocity oldVelocity,
                                    final Acceleration fx,
                                    final Acceleration fy,
                                    final Acceleration fz,
                                    final AngularSpeed angularRateX,
                                    final AngularSpeed angularRateY,
                                    final AngularSpeed angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                convertAccelerationToDouble(fx), convertAccelerationToDouble(fy),
                convertAccelerationToDouble(fz),
                convertAngularSpeedToDouble(angularRateX),
                convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/indoor/Utils.java 2105
com/irurueta/navigation/indoor/Utils.java 2334
                }

                @Override
                public int getNumberOfVariables() {
                    return 1;
                }
            };

            final JacobianEstimator jacobianEstimator = new JacobianEstimator(evaluator);

            return MultivariateNormalDist.propagate(new MultivariateNormalDist.JacobianEvaluator() {
                @Override
                public void evaluate(
                        final double[] x, final double[] y, final Matrix jacobian) {

                    try {
                        evaluator.evaluate(x, y);
                        jacobianEstimator.jacobian(x, jacobian);
                    } catch (final EvaluationException ignore) {
                        //never happens
                    }
                }

                @Override
                public int getNumberOfVariables() {
                    return 1;
                }
            }, mean, covariance);
        } catch (final AlgebraException | StatisticsException e) {
            throw new IndoorException(e);
        }
    }

    /**
     * Propagates provided variances (fingerprint rssi variance, path-loss exponent variance,
     * fingerprint position covariance and radio source position covariance) into
     * rssi variance by considering the 3D 3rd order Taylor expression of received power.
     * Notice that any unknown variance is assumed to be zero.
     *
     * @param fingerprintRssi               closest located fingerprint reading RSSI expressed in dBm's.
     * @param pathLossExponent              path-loss exponent.
     * @param fingerprintPosition           position of closest fingerprint.
     * @param radioSourcePosition           radio source position associated to fingerprint reading.
     * @param estimatedPosition             position to be estimated. Usually this is equal to the
     *                                      initial position used by a non linear algorithm.
     * @param fingerprintRssiVariance       variance of fingerprint RSSI or null if unknown.
     * @param pathLossExponentVariance      variance of path-loss exponent or null if unknown.
     * @param fingerprintPositionCovariance covariance of fingerprint position or null if
     *                                      unknown.
     * @param radioSourcePositionCovariance covariance of radio source position or null
     *                                      if unknown.
     * @param estimatedPositionCovariance   covariance of position to be estimated or null
     *                                      if unknown. (This is usually unknown).
     * @return a normal distribution containing expected received RSSI value and its variance.
     * @throws IndoorException if something fails.
     */
    public static MultivariateNormalDist propagateVariancesToRssiVarianceThirdOrderNonLinear3D(
File Line
com/irurueta/navigation/inertial/INSGNSSLooselyCoupledKalmanFilteredEstimator.java 1600
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1552
    }

    /**
     * Indicates whether provided measurements are ready to be
     * used for an update.
     *
     * @param measurements measurements to be checked.
     * @return true if estimator is ready, false otherwise.
     */
    public static boolean isUpdateMeasurementsReady(
            final Collection<GNSSMeasurement> measurements) {
        return GNSSLeastSquaresPositionAndVelocityEstimator
                .isValidMeasurements(measurements);
    }

    /**
     * Updates GNSS measurements of this estimator when new satellite measurements
     * are available.
     * Calls to this method will be ignored if interval between provided timestamp
     * and last timestamp when Kalman filter was updated is less than epoch interval.
     *
     * @param measurements GNSS measurements to be updated.
     * @param timestamp    timestamp since epoch time when GNSS measurements were
     *                     updated.
     * @return true if measurements were updated, false otherwise.
     * @throws LockedException   if this estimator is already running.
     * @throws NotReadyException if estimator is not ready for measurements updates.
     * @throws INSGNSSException  if estimation fails due to numerical instabilities.
     */
    public boolean updateMeasurements(
            final Collection<GNSSMeasurement> measurements, final Time timestamp)
            throws LockedException, NotReadyException, INSGNSSException {
        return updateMeasurements(measurements, TimeConverter.convert(
                timestamp.getValue().doubleValue(), timestamp.getUnit(),
                TimeUnit.SECOND));
    }

    /**
     * Updates GNSS measurements of this estimator when new satellite measurements
     * are available.
     * Call to this method will be ignored if interval between provided timestamp
     * and last timestamp when Kalman filter was updated is less than epoch interval.
     *
     * @param measurements GNSS measurements to be updated.
     * @param timestamp    timestamp expressed in seconds since epoch time when
     *                     GNSS measurements were updated.
     * @return true if measurements were updated, false otherwise.
     * @throws LockedException   if this estimator is already running.
     * @throws NotReadyException if estimator is not ready for measurements updates.
     * @throws INSGNSSException  if estimation fails due to numerical instabilities.
     */
    public boolean updateMeasurements(
            final Collection<GNSSMeasurement> measurements, final double timestamp)
            throws LockedException, NotReadyException, INSGNSSException {

        if (mRunning) {
            throw new LockedException();
        }

        if (!isUpdateMeasurementsReady(measurements)) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 7235
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5537
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5885
            getBiasAsMatrix(mBa);

            mFmeas.setElementAtIndex(0, mFmeasX);
            mFmeas.setElementAtIndex(1, mFmeasY);
            mFmeas.setElementAtIndex(2, mFmeasZ);

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1577
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1683
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 353
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1701
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1352
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1585
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1320
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 359
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 359
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 359
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2065
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2062
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1575
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1679
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 350
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1697
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1348
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1580
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1316
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 355
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 354
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 355
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2061
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2058
        this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return mQualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && mQualityScores != null &&
                mQualityScores.length == mMeasurements.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3348
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3519
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4836
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5040
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        final Matrix g = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        g.setElementAtIndex(0, g11);
        g.setElementAtIndex(1, g21);
        g.setElementAtIndex(2, g31);

        g.setElementAtIndex(3, g12);
        g.setElementAtIndex(4, g22);
        g.setElementAtIndex(5, g32);

        g.setElementAtIndex(6, g13);
        g.setElementAtIndex(7, g23);
        g.setElementAtIndex(8, g33);

        setResult(m, b, g);
    }

    /**
     * Internal method to perform general calibration when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneralAndGDependentCrossBiases()
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5541
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5641
            mFmeas.setElementAtIndex(2, mFmeasZ);

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);

            mB.setElementAtIndex(0, mBiasX);
            mB.setElementAtIndex(1, mBiasY);
            mB.setElementAtIndex(2, mBiasZ);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5999
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 2801
            final double angularRateMeasZ2 = mBiasZ + m1.getElementAtIndex(2);

            final double diffX = angularRateMeasX2 - angularRateMeasX1;
            final double diffY = angularRateMeasY2 - angularRateMeasY1;
            final double diffZ = angularRateMeasZ2 - angularRateMeasZ1;

            return Math.sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ);

        } catch (final WrongSizeException e) {
            return Double.MAX_VALUE;
        }
    }

    /**
     * Computes a preliminary solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminary solution will be stored.
     */
    protected void computePreliminarySolutions(
            final int[] samplesIndices, final List<PreliminaryResult> solutions) {

        final List<StandardDeviationFrameBodyKinematics> measurements = new ArrayList<>();

        for (final int samplesIndex : samplesIndices) {
            measurements.add(mMeasurements.get(samplesIndex));
        }

        try {
            final PreliminaryResult result = new PreliminaryResult();
            result.mEstimatedMg = getInitialMg();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4808
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7497
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6130
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8907
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5040
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7874
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6422
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9361
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5889
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5993
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2906
            mFmeas.setElementAtIndex(2, mFmeasZ);

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);

            mB.setElementAtIndex(0, bx);
            mB.setElementAtIndex(1, by);
            mB.setElementAtIndex(2, bz);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 7237
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7903
            mFmeas.setElementAtIndex(0, mFmeasX);
            mFmeas.setElementAtIndex(1, mFmeasY);
            mFmeas.setElementAtIndex(2, mFmeasZ);

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7907
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4266
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5995
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2908
            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);

            mB.setElementAtIndex(0, bx);
            mB.setElementAtIndex(1, by);
            mB.setElementAtIndex(2, bz);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1016
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1045
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
        this(measurements, commonAxisUsed, listener);
        internalSetBias(bias);
    }

    /**
     * Gets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return x coordinate of accelerometer bias.
     */
    public double getBiasX() {
        return mBiasX;
    }

    /**
     * Sets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasX x coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final double biasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
    }

    /**
     * Gets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return y coordinate of accelerometer bias.
     */
    public double getBiasY() {
        return mBiasY;
    }

    /**
     * Sets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasY y coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final double biasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasY = biasY;
    }

    /**
     * Gets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return z coordinate of accelerometer bias.
     */
    public double getBiasZ() {
        return mBiasZ;
    }

    /**
     * Sets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final double biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mBiasZ = biasZ;
    }

    /**
     * Gets known x coordinate of accelerometer bias.
     *
     * @return x coordinate of accelerometer bias.
     */
    public Acceleration getBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2708
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3005
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4043
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4790
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias. This must have length 3 and is
     *                      expressed in meters per squared second (m/s^2).
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4183
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4367
            final boolean commonAxisUsed, final double[] bias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4318
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5068
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4365
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5115
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4460
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5210
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final Matrix initialMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, initialMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, initialMa);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param initialMa     initial scale factors and cross coupling errors matrix.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4932
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5117
            final boolean commonAxisUsed, final double[] bias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4243
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5013
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial accelerometer bias to be used to find a solution.
     *                      This must have length 3 and is expressed in meters per
     *                      squared second (m/s^2).
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4388
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4580
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4529
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5300
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4578
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5349
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4678
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5449
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        initialMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        initialMa);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param initialMa     initial scale factors and cross coupling errors matrix.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5158
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5351
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2779
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3080
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5978
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 12528
            final Matrix m1 = Matrix.identity(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            m1.add(mg);

            final Matrix angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            angularRateTrue.setElementAtIndex(0, angularRateTrueX);
            angularRateTrue.setElementAtIndex(1, angularRateTrueY);
            angularRateTrue.setElementAtIndex(2, angularRateTrueZ);

            m1.multiply(angularRateTrue);

            final Matrix fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            fTrue.setElementAtIndex(0, fTrueX);
            fTrue.setElementAtIndex(1, fTrueY);
            fTrue.setElementAtIndex(2, fTrueZ);
            final Matrix m2 = gg.multiplyAndReturnNew(fTrue);

            m1.add(m2);

            final double angularRateMeasX2 = mBiasX + m1.getElementAtIndex(0);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 11938
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 2780
            final Matrix m1 = Matrix.identity(BodyKinematics.COMPONENTS,
                    BodyKinematics.COMPONENTS);
            m1.add(mg);

            final Matrix angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            angularRateTrue.setElementAtIndex(0, angularRateTrueX);
            angularRateTrue.setElementAtIndex(1, angularRateTrueY);
            angularRateTrue.setElementAtIndex(2, angularRateTrueZ);

            m1.multiply(angularRateTrue);

            final Matrix fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            fTrue.setElementAtIndex(0, fTrueX);
            fTrue.setElementAtIndex(1, fTrueY);
            fTrue.setElementAtIndex(2, fTrueZ);
            final Matrix m2 = gg.multiplyAndReturnNew(fTrue);

            m1.add(m2);

            final double angularRateMeasX2 = mBiasX + m1.getElementAtIndex(0);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2733
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2904
            mBmeas.setElementAtIndex(0, mBmeasX);
            mBmeas.setElementAtIndex(1, mBmeasY);
            mBmeas.setElementAtIndex(2, mBmeasZ);

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4657
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5504
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4809
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5008
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4955
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5805
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5006
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5853
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5112
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5959
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron, final Matrix initialMm,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        initialMm);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        initialMm);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param initialMm     initial soft-iron matrix containing scale factors
     *                      and cross coupling errors.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5656
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5855
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4766
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5613
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4918
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5117
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5064
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5914
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5115
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5962
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5221
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6068
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron, final Matrix initialMm,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        initialMm);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        initialMm);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5765
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5964
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator2D.java 2975
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator3D.java 2956
        final Point2D initialPosition = result.getEstimatedPosition();
        final double initialTransmittedPowerdBm =
                result.getEstimatedTransmittedPowerdBm();
        final double initialPathLossExponent = result.getEstimatedPathLossExponent();

        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mReadings.size();

            mInnerReadings.clear();

            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    //sample is inlier
                    mInnerReadings.add(mReadings.get(i));
                }
            }

            try {
                mInnerEstimator.setInitialPosition(initialPosition);
                mInnerEstimator.setInitialTransmittedPowerdBm(initialTransmittedPowerdBm);
                mInnerEstimator.setInitialPathLossExponent(initialPathLossExponent);
                mInnerEstimator.setTransmittedPowerEstimationEnabled(
                        mTransmittedPowerEstimationEnabled);
                mInnerEstimator.setPathLossEstimationEnabled(
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator3D.java 2982
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator2D.java 2955
        final Point3D initialPosition = result.getEstimatedPosition();
        final double initialTransmittedPowerdBm =
                result.getEstimatedTransmittedPowerdBm();
        final double initialPathLossExponent = result.getEstimatedPathLossExponent();

        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mReadings.size();

            mInnerReadings.clear();

            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    //sample is inlier
                    mInnerReadings.add(mReadings.get(i));
                }
            }

            try {
                mInnerEstimator.setInitialPosition(initialPosition);
                mInnerEstimator.setInitialTransmittedPowerdBm(initialTransmittedPowerdBm);
                mInnerEstimator.setInitialPathLossExponent(initialPathLossExponent);
                mInnerEstimator.setTransmittedPowerEstimationEnabled(
                        mTransmittedPowerEstimationEnabled);
                mInnerEstimator.setPathLossEstimationEnabled(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3771
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4443
                jacobian.setElementAt(2, 11, ftruey);
            }
        });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double sx = result[3];
        final double sy = result[4];
        final double sz = result[5];

        final double mxy = result[6];
        final double mxz = result[7];
        final double myx = result[8];
        final double myz = result[9];
        final double mzx = result[10];
        final double mzy = result[11];

        if (mEstimatedBiases == null) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7547
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3468
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5346
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2536
                        return evaluateGeneral(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];
        final double m21 = result[4];
        final double m31 = result[5];

        final double m12 = result[6];
        final double m22 = result[7];
        final double m32 = result[8];

        final double m13 = result[9];
        final double m23 = result[10];
        final double m33 = result[11];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 839
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 216
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 847
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 763
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1885
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 223
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1915
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 204
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 981
        final MSACRobustEstimator<PreliminaryResult> innerEstimator =
                new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 912
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 292
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 924
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 836
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1960
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 299
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1990
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 292
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1055
        final RANSACRobustEstimator<PreliminaryResult> innerEstimator =
                new RANSACRobustEstimator<>(
                        new RANSACRobustEstimatorListener<PreliminaryResult>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mMeasurements.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return mPreliminarySubsetSize;
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<PreliminaryResult> solutions) {
                                computePreliminarySolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final PreliminaryResult currentEstimation,
                                    final int i) {
                                return computeError(mMeasurements.get(i), currentEstimation);
                            }

                            @Override
                            public boolean isReady() {
                                return RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3971
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4324
            final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4231
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5164
            final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4414
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4980
            final boolean commonAxisUsed, final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias,
                        listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param initialMa     initial scale factors and cross coupling errors matrix.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4439
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5400
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4629
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5209
            final boolean commonAxisUsed, final Matrix initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param initialMa     initial scale factors and cross coupling errors matrix.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3125
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3283
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4586
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4774
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        final Matrix g = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        g.setElementAtIndex(0, g11);
        g.setElementAtIndex(1, g21);
        g.setElementAtIndex(2, g31);

        g.setElementAtIndex(3, g12);
        g.setElementAtIndex(4, g22);
        g.setElementAtIndex(5, g32);

        g.setElementAtIndex(6, g13);
        g.setElementAtIndex(7, g23);
        g.setElementAtIndex(8, g33);

        setResult(m, g);
    }

    /**
     * Internal method to perform general calibration when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneralAndGDependentCrossBiases()
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4045
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4403
            final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores, final double biasX, final double biasY,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4673
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7352
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4808
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8907
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5995
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8760
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6130
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7497
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4893
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7715
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5040
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9361
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6280
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9205
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6422
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7874
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4861
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5906
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5059
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5708
            final boolean commonAxisUsed, final Matrix hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param initialMm     initial soft-iron matrix containing scale factors
     *                      and cross coupling errors.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4970
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6015
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5168
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5817
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator2D.java 428
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator2D.java 440
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 512
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator2D.java 276
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator2D.java 523
                        new MSACRobustEstimatorListener<Solution<Point2D>>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point2D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point2D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return MSACRobustRangingAndRssiRadioSourceEstimator2D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator3D.java 428
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator3D.java 442
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator3D.java 512
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator3D.java 276
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 521
                        new MSACRobustEstimatorListener<Solution<Point3D>>() {
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point3D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point3D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return MSACRobustRangingAndRssiRadioSourceEstimator3D.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 665
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 738
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 650
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 200
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 181
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 958
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(
                        new LMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5655
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6977
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8606
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10020
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5850
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7167
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8405
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9816
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, bias, initialMg,
                        initialGg, accelerometerBias, accelerometerMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5931
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7318
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9044
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10540
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6133
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7525
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8828
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10321
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator.java 524
com/irurueta/navigation/indoor/radiosource/RangingAndRssiRadioSourceEstimator.java 519
com/irurueta/navigation/indoor/radiosource/RssiRadioSourceEstimator.java 521
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator.java 1639
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator.java 1619
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPowerdBm() {
        return mInitialTransmittedPowerdBm;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in dBm's).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @param initialTransmittedPowerdBm initial transmitted power to start the
     *                                   estimation of radio source transmitted
     *                                   power.
     * @throws LockedException if estimator is locked.
     */
    public void setInitialTransmittedPowerdBm(final Double initialTransmittedPowerdBm)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mInitialTransmittedPowerdBm = initialTransmittedPowerdBm;
    }

    /**
     * Gets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @return initial transmitted power to start the estimation of radio source
     * transmitted power.
     */
    public Double getInitialTransmittedPower() {
        return mInitialTransmittedPowerdBm != null ?
                Utils.dBmToPower(mInitialTransmittedPowerdBm) : null;
    }

    /**
     * Sets initial transmitted power to start the estimation of radio source
     * transmitted power (expressed in mW).
     * If not defined, average value of received power readings will be used.
     * <p>
     * If transmitted power estimation is enabled, estimation will start at this
     * value and will converte the most appropriate value.
     * If transmitted power estimation is disabled, this value will be assumed to be
     * exact and the estimated transmitted power will be equal to this value
     * (converted to dBm's).
     *
     * @param initialTransmittedPower initial transmitted power to start the
     *                                estimation of radio source transmitted power.
     * @throws LockedException          if estimator is locked.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setInitialTransmittedPower(final Double initialTransmittedPower)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (initialTransmittedPower != null) {
            if (initialTransmittedPower < 0.0) {
                throw new IllegalArgumentException();
            }
            mInitialTransmittedPowerdBm = Utils.powerTodBm(
                    initialTransmittedPower);
        } else {
            mInitialTransmittedPowerdBm = null;
        }
    }

    /**
     * Indicates whether transmitted power estimation is enabled or not.
     *
     * @return true if transmitted power estimation is enabled, false otherwise.
     */
    public boolean isTransmittedPowerEstimationEnabled() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6922
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5041
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2407
                        mFmeasZ = point[2];

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateGeneral(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double m11 = result[0];
        final double m21 = result[1];
        final double m31 = result[2];

        final double m12 = result[3];
        final double m22 = result[4];
        final double m32 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3771
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4240
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4443
                jacobian.setElementAt(2, 11, ftruey);
            }
        });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double sx = result[3];
        final double sy = result[4];
        final double sz = result[5];

        final double mxy = result[6];
        final double mxz = result[7];
        final double myx = result[8];
        final double myz = result[9];
        final double mzx = result[10];
        final double mzy = result[11];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7706
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5183
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2701
                        mFmeasZ = point[2];

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateCommonAxis(params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];

        final double m12 = result[4];
        final double m22 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 458
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 487
            final RobustKnownFrameAccelerometerCalibratorListener listener) {
        this(measurements, commonAxisUsed);
        mListener = listener;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solutions.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    public Acceleration getInitialBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3991
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3712
        if (mEstimatedMg == null) {
            mEstimatedMg = m;
        } else {
            mEstimatedMg.copyFrom(m);
        }

        for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
            mEstimatedMg.setElementAt(i, i,
                    mEstimatedMg.getElementAt(i, i) - 1.0);
        }

        if (mEstimatedGg == null) {
            mEstimatedGg = new Matrix(
                    BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            mEstimatedGg.initialize(0.0);
        }

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Computes gravity versor error at the end of a sequence using provided
     * parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param i      row position.
     * @param params array containing parameters for the general purpose case
     *               when G-dependent cross biases are taken into account. Must
     *               have length 18.
     * @return error between estimated and measured gravity versor.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneralWithGDependentCrossBiases(
            final int i, final double[] params)
            throws EvaluationException {

        final double bx = params[0];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3792
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3830
                jacobian.setElementAt(0, 7, 0.0);
                jacobian.setElementAt(0, 8, 0.0);
                jacobian.setElementAt(0, 9, ftruex);
                jacobian.setElementAt(0, 10, ftruey);
                jacobian.setElementAt(0, 11, ftruez);
                jacobian.setElementAt(0, 12, 0.0);
                jacobian.setElementAt(0, 13, 0.0);
                jacobian.setElementAt(0, 14, 0.0);
                jacobian.setElementAt(0, 15, 0.0);
                jacobian.setElementAt(0, 16, 0.0);
                jacobian.setElementAt(0, 17, 0.0);

                jacobian.setElementAt(1, 0, 0.0);
                jacobian.setElementAt(1, 1, omegatruey);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3812
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3850
                jacobian.setElementAt(1, 8, 0.0);
                jacobian.setElementAt(1, 9, 0.0);
                jacobian.setElementAt(1, 10, 0.0);
                jacobian.setElementAt(1, 11, 0.0);
                jacobian.setElementAt(1, 12, ftruex);
                jacobian.setElementAt(1, 13, ftruey);
                jacobian.setElementAt(1, 14, ftruez);
                jacobian.setElementAt(1, 15, 0.0);
                jacobian.setElementAt(1, 16, 0.0);
                jacobian.setElementAt(1, 17, 0.0);

                jacobian.setElementAt(2, 0, 0.0);
                jacobian.setElementAt(2, 1, 0.0);
                jacobian.setElementAt(2, 2, omegatruez);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4673
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8760
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5995
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7352
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4893
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9205
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6280
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7715
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3711
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3670
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2079
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2095
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException |
                com.irurueta.numerical.NotReadyException |
                IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2318
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2347
    public static void estimateKinematics(final double timeInterval,
                                          final ECEFFrame frame,
                                          final CoordinateTransformation oldC,
                                          final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                          final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC,
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param frame        body ECEF frame containing current position, velocity and
     *                     body-to-ECEF frame coordinate transformation.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2319
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1277
                                          final ECEFFrame frame,
                                          final CoordinateTransformation oldC,
                                          final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                          final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC,
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param frame        body ECEF frame containing current position, velocity and
     *                     body-to-ECEF frame coordinate transformation.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final Time timeInterval,
                                          final ECEFFrame frame,
File Line
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1276
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1305
    public static void estimateKinematics(final double timeInterval,
                                          final ECIFrame frame,
                                          final CoordinateTransformation oldC,
                                          final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                          final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC,
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param frame        body ECI frame containing current position, velocity and
     *                     body-to-ECI frame coordinate transformation.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        x coordinate of previous velocity of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldVy        y coordinate of previous velocity of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldVz        z coordinate of previous velocity of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param result       instance where estimated body kinematics will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
     *                                  are not ECI frame valid.
     */
    public static void estimateKinematics(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10370
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10417
    public static void navigateECEF(final double timeInterval,
                                    final ECEFPosition oldPosition,
                                    final CoordinateTransformation oldC,
                                    final ECEFVelocity oldVelocity,
                                    final double fx,
                                    final double fy,
                                    final double fz,
                                    final AngularSpeed angularRateX,
                                    final AngularSpeed angularRateY,
                                    final AngularSpeed angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                fx, fy, fz, convertAngularSpeedToDouble(angularRateX),
                convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 7039
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3378
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4879
                        return BodyKinematics.COMPONENTS;
                    }

                    @Override
                    public double[] createInitialParametersArray() {
                        final double[] initial = new double[COMMON_Z_AXIS_UNKNOWNS];

                        // upper diagonal cross coupling errors M
                        int k = 0;
                        for (int j = 0; j < BodyKinematics.COMPONENTS; j++) {
                            for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
                                if (i <= j) {
                                    initial[k] = initialM.getElementAt(i, j);
                                    k++;
                                }
                            }
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(final int i, final double[] point,
                                           final double[] params, final double[] derivatives)
                            throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2751
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3045
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3879
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4238
            final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, biasX, biasY, biasZ, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, biasX, biasY, biasZ, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param measurements  list of body kinematics measurements with standard
     *                      deviations taken at different frames (positions, orientations
     *                      and velocities).
     * @param biasX         known x coordinate of accelerometer bias expressed in meters per
     *                      squared second (m/s^2).
     * @param biasY         known y coordinate of accelerometer bias expressed in meters per
     *                      squared second (m/s^2).
     * @param biasZ         known z coordinate of accelerometer bias expressed in meters per
     *                      squared second (m/s^2).
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores,
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3660
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4691
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2822
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3120
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3953
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4317
            final double biasZ,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX,
                        biasY, biasZ, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(biasX,
                        biasY, biasZ, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX,
                        biasY, biasZ, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, biasX, biasY, biasZ, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, biasX, biasY, biasZ, listener);
        }
    }

    /**
     * Cretes a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param measurements  list of body kinematics measurements with standard
     *                      deviations taken at different frames (positions, orientations
     *                      and velocities).
     * @param biasX         known x coordinate of gyroscope bias expressed in radians per
     *                      second (rad/s).
     * @param biasY         known y coordinate of gyroscope bias expressed in radians per
     *                      second (rad/s).
     * @param biasZ         known z coordinate of gyroscope bias expressed in radians per
     *                      second (rad/s).
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3454
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4485
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 918
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 923
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 410
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 409
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 935
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 439
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator3D.java 439
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator2D.java 204
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator3D.java 204
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator2D.java 450
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 449
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2698
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6584
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2965
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7023
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3078
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3224
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException |
                com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1646
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1419
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1387
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 426
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2132
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 2774
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 6051
                result.mEstimatedMa = mNonLinearCalibrator.getEstimatedMa();
            }

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mMeasurements.size();

            final List<StandardDeviationFrameBodyKinematics> inlierMeasurements =
                    new ArrayList<>();
            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(mMeasurements.get(i));
                }
            }

            try {
                mNonLinearCalibrator.setInitialBias(preliminaryResult.mEstimatedBiases);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 93
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 124
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD =
            RobustEstimatorMethod.LMedS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen subsamples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Known x-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasX;

    /**
     * Known y-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasY;

    /**
     * Known z-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasZ;

    /**
     * Known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSx;

    /**
     * Known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSy;

    /**
     * Known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSz;

    /**
     * Known accelerometer x-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMxy;

    /**
     * Know accelerometer x-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMxz;

    /**
     * Known accelerometer y-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMyx;

    /**
     * Known accelerometer y-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMyz;

    /**
     * Known accelerometer z-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMzx;

    /**
     * Known accelerometer z-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMzy;

    /**
     * Initial x-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     */
    private double mInitialBiasX;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 93
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 124
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD =
            RobustEstimatorMethod.LMedS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen subsamples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Known x-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasX;

    /**
     * Known y-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasY;

    /**
     * Known z-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double mAccelerometerBiasZ;

    /**
     * Known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSx;

    /**
     * Known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSy;

    /**
     * Known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerSz;

    /**
     * Known accelerometer x-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMxy;

    /**
     * Know accelerometer x-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMxz;

    /**
     * Known accelerometer y-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMyx;

    /**
     * Known accelerometer y-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMyz;

    /**
     * Known accelerometer z-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMzx;

    /**
     * Known accelerometer z-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double mAccelerometerMzy;

    /**
     * X-coordinate of gyroscope known bias expressed in radians per second
     * (rad/s).
     */
    private double mBiasX;
File Line
com/irurueta/navigation/lateration/HomogeneousLinearLeastSquaresLaterationSolver.java 71
com/irurueta/navigation/lateration/InhomogeneousLinearLeastSquaresLaterationSolver.java 76
    public HomogeneousLinearLeastSquaresLaterationSolver(
            final P[] positions, final double[] distances,
            final LaterationSolverListener<P> listener) {
        super(positions, distances, listener);
    }

    /**
     * Solves the lateration problem.
     *
     * @throws LaterationException if lateration fails.
     * @throws NotReadyException   if solver is not ready.
     * @throws LockedException     if instance is busy solving the lateration problem.
     */
    @Override
    @SuppressWarnings("Duplicates")
    public void solve() throws LaterationException, NotReadyException,
            LockedException {
        // The implementation on this method follows the algorithm  bellow.

        // Having 3 2D circles:
        // c1x, c1y, r1
        // c2x, c2y, r2
        // c3x, c3y, r3
        // where (c1x, c1y) are the coordinates of 1st circle center and r1 is its radius.
        // (c2x, c2y) are the coordinates of 2nd circle center and r2 is its radius.
        // (c3x, c3y) are the coordinates of 3rd circle center and r3 is its radius.

        // The equations of the circles are as follows:
        // (x - c1x)^2 + (y - c1y)^2 = r1^2
        // (x - c2x)^2 + (y - c2y)^2 = r2^2
        // (x - c3x)^2 + (y - c3y)^2 = r3^2

        // x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 = r1^2
        // x^2 - 2*c2x*x + c2x^2 + y^2 - 2*c2y*y + c2y^2 = r2^2
        // x^2 - 2*c3x*x + c3x^2 + y^2 - 2*c3y*y + c3y^2 = r3^2

        // remove 1st equation from others (we use 1st point as reference)

        // x^2 - 2*c2x*x + c2x^2 + y^2 - 2*c2y*y + c2y^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2) = r2^2 - r1^2
        // x^2 - 2*c3x*x + c3x^2 + y^2 - 2*c3y*y + c3y^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2) = r3^2 - r1^2

        // - 2*c2x*x + c2x^2 - 2*c2y*y + c2y^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 = r2^2 - r1^2
        // - 2*c3x*x + c3x^2 - 2*c3y*y + c3y^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 = r3^2 - r1^2

        // 2*(c1x - c2x)*x + c2x^2 + 2*(c1y - c2y)*y + c2y^2 - c1x^2 - c1y^2 = r2^2 - r1^2
        // 2*(c1x - c3x)*x + c3x^2 + 2*(c1y - c3y)*y + c3y^2 - c1x^2 - c1y^2 = r3^2 - r1^2

        // 2*(c1x - c2x)*x + 2*(c1y - c2y)*y = r2^2 - r1^2 + c1x^2 + c1y^2 - c2x^2 - c2y^2
        // 2*(c1x - c3x)*x + 2*(c1y - c3y)*y = r3^2 - r1^2 + c1x^2 + c1y^2 - c3x^2 - c3y^2

        // x and y are the inhomogeneous coordinates of the point (x,y) we want to find, we
        // substitute such point by the corresponding homogeneous coordinates (x,y) = (x'/w', y'/w')

        // Hence
        // 2*(c1x - c2x)*x'/w' + 2*(c1y - c2y)*y'/w' = r2^2 - r1^2 + c1x^2 + c1y^2 - c2x^2 - c2y^2
        // 2*(c1x - c3x)*x'/w' + 2*(c1y - c3y)*y'/w' = r3^2 - r1^2 + c1x^2 + c1y^2 - c3x^2 - c3y^2

        // Multiplitying by w' at both sides...
        // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' = (r2^2 - r1^2 + c1x^2 + c1y^2 - c2x^2 - c2y^2)*w'
        // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' = (r3^2 - r1^2 + c1x^2 + c1y^2 - c3x^2 - c3y^2)*w'

        // Obtaining the following homogeneous equations
        // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' - (r2^2 - r1^2 + c1x^2 + c1y^2 - c2x^2 - c2y^2)*w' = 0
        // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' - (r3^2 - r1^2 + c1x^2 + c1y^2 - c3x^2 - c3y^2)*w' = 0

        // Fixing signs...
        // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' + (r1^2 - r2^2 + c2x^2 + c2y^2 - c1x^2 - c1y^2)*w' = 0
        // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' + (r1^2 - r3^2 + c3x^2 + c3y^2 - c1x^2 - c1y^2)*w' = 0


        // The homogeneous equations can be expressed as a linear system of homogeneous equations A*x = 0
        // where the unknowns to be solved are (x', y', w') up to scale.

        // [2*(c1x - c2x)   2*(c1y - c2y)    r1^2 - r2^2 + c2x^2 + c2y^2 - c1x^2 - c1y^2][x'] = 0
        // [2*(c1x - c3x)   2*(c1y - c3y)    r1^2 - r3^2 + c3x^2 + c3y^2 - c1x^2 - c1y^2][y'] = 0
        //                                                                               [w']

        // This can be solved by using the SVD decomposition of matrix A and picking the last column of
        // resulting V matrix. At least 2 equations are required to find a solution, since 1 additional
        // point is used as a reference, at least 3 points are required.

        // For spheres the solution is analogous

        // Having 4 3D spheres:
        // c1x, c1y, c1z, r1
        // c2x, c2y, c2z, r2
        // c3x, c3y, c3z, r3
        // c4x, c4y, c4z, r4
        // where (c1x, c1y, c1z) are the coordinates of 1st sphere center and r1 is its radius.
        // (c2x, c2y, c2z) are the coordinates of 2nd sphere center and r2 is its radius.
        // (c3x, c3y, c3z) are the coordinates of 3rd sphere center and r3 is its radius.
        // (c4x, c4y, c4z) are the coordinates of 4th sphere center and r4 is its radius.

        // The equations of the spheres are as follows:
        // (x - c1x)^2 + (y - c1y)^2 + (z - c1z)^2 = r1^2
        // (x - c2x)^2 + (y - c2y)^2 + (z - c2z)^2 = r2^2
        // (x - c3x)^2 + (y - c3y)^2 + (z - c3z)^2 = r3^2
        // (x - c4x)^2 + (y - c4y)^2 + (z - c4z)^2 = r4^2

        // x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 + z^2 - 2*c1z*z + c1z^2 = r1^2
        // x^2 - 2*c2x*x + c2x^2 + y^2 - 2*c2y*y + c2y^2 + z^2 - 2*c2z*z + c2z^2 = r2^2
        // x^2 - 2*c3x*x + c3x^2 + y^2 - 2*c3y*y + c3y^3 + z^2 - 2*c3z*z + c3z^2 = r3^2
        // x^2 - 2*c4x*x + c4x^2 + y^2 - 2*c4y*y + c4y^2 + z^2 - 2*c4z*z + c4z^2 = r4^2

        // remove 1st equation from others (we use 1st point as reference)
        // x^2 - 2*c2x*x + c2x^2 + y^2 - 2*c2y*y + c2y^2 + z^2 - 2*c2z*z + c2z^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 + z^2 - 2*c1z*z + c1z^2) = r2^2 - r1^2
        // x^2 - 2*c3x*x + c3x^2 + y^2 - 2*c3y*y + c3y^3 + z^2 - 2*c3z*z + c3z^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 + z^2 - 2*c1z*z + c1z^2) = r3^2 - r1^2
        // x^2 - 2*c4x*x + c4x^2 + y^2 - 2*c4y*y + c4y^2 + z^2 - 2*c4z*z + c4z^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 + z^2 - 2*c1z*z + c1z^2) = r4^2 - r1^2

        // - 2*c2x*x + c2x^2 - 2*c2y*y + c2y^2 - 2*c2z*z + c2z^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 + 2*c1z*z - c1z^2 = r2^2 - r1^2
        // - 2*c3x*x + c3x^2 - 2*c3y*y + c3y^3 - 2*c3z*z + c3z^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 + 2*c1z*z - c1z^2 = r3^2 - r1^2
        // - 2*c4x*x + c4x^2 - 2*c4y*y + c4y^2 - 2*c4z*z + c4z^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 + 2*c1z*z - c1z^2 = r4^2 - r1^2

        // 2*(c1x - c2x)*x + c2x^2 + 2*(c1y - c2y)*y + c2y^2 + 2*(c1z - c2z)*z + c2z^2 - c1x^2 - c1y^2 - c1z^2 = r2^2 - r1^2
        // 2*(c1x - c3x)*x + c3x^2 + 2*(c1y - c3y)*y + c3y^3 + 2*(c1z - c3z)*z + c3z^2 - c1x^2 - c1y^2 - c1z^2 = r3^2 - r1^2
        // 2*(c1x - c4x)*x + c4x^2 + 2*(c1y - c4y)*y + c4y^2 + 2*(c1z - c4z)*z + c4z^2 - c1x^2 - c1y^2 - c1z^2 = r4^2 - r1^2

        // 2*(c1x - c2x)*x + 2*(c1y - c2y)*y + 2*(c1z - c2z)*z = r2^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c2x^2 - c2y^2 - c2z^2
        // 2*(c1x - c3x)*x + 2*(c1y - c3y)*y + 2*(c1z - c3z)*z = r3^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c3x^2 - c3y^3 - c3z^2
        // 2*(c1x - c4x)*x + 2*(c1y - c4y)*y + 2*(c1z - c4z)*z = r4^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c4x^2 - c4y^2 - c4z^2

        // x, y and z the inhomogeneous coordinates of the point (x,y,z) we want to find,
        // we substitute such point by the corresponding homogeneous coordinates
        // (x, y, z) = (x'/w', y'/w', z'/w')

        // Hence
        // 2*(c1x - c2x)*x'/w' + 2*(c1y - c2y)*y'/w' + 2*(c1z - c2z)*z'/w' = r2^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c2x^2 - c2y^2 - c2z^2
        // 2*(c1x - c3x)*x'/w' + 2*(c1y - c3y)*y'/w' + 2*(c1z - c3z)*z'/w' = r3^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c3x^2 - c3y^3 - c3z^2
        // 2*(c1x - c4x)*x'/w' + 2*(c1y - c4y)*y'/w' + 2*(c1z - c4z)*z'/w' = r4^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c4x^2 - c4y^2 - c4z^2

        // Multipliying by w' at both sides...
        // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' + 2*(c1z - c2z)*z' = (r2^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c2x^2 - c2y^2 - c2z^2)*w'
        // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' + 2*(c1z - c3z)*z' = (r3^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c3x^2 - c3y^3 - c3z^2)*w'
        // 2*(c1x - c4x)*x' + 2*(c1y - c4y)*y' + 2*(c1z - c4z)*z' = (r4^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c4x^2 - c4y^2 - c4z^2)*w'

        // Obtaining the following homogeneous equations
        // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' + 2*(c1z - c2z)*z' - (r2^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c2x^2 - c2y^2 - c2z^2)*w' = 0
        // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' + 2*(c1z - c3z)*z' - (r3^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c3x^2 - c3y^3 - c3z^2)*w' = 0
        // 2*(c1x - c4x)*x' + 2*(c1y - c4y)*y' + 2*(c1z - c4z)*z' - (r4^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c4x^2 - c4y^2 - c4z^2)*w' = 0

        // Fixing signs...
        // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' + 2*(c1z - c2z)*z' + (r1^2 - r2^2 + c2x^2 + c2y^2 + c2z^2 - c1x^2 - c1y^2 - c1z^2)*w' = 0
        // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' + 2*(c1z - c3z)*z' + (r1^2 - r3^2 + c3x^2 + c3y^3 + c3z^2 - c1x^2 - c1y^2 - c1z^2)*w' = 0
        // 2*(c1x - c4x)*x' + 2*(c1y - c4y)*y' + 2*(c1z - c4z)*z' + (r1^2 - r4^2 + c4x^2 + c4y^2 + c4z^2 - c1x^2 - c1y^2 - c1z^2)*w' = 0

        // The homogeneous equastions can be expressed as a linear system of homogeneous equations
        // where the unknowns to be solved are (x', y', z', w') up to scale.

        // [2*(c1x - c2x)   2*(c1y - c2y)   2*(c1z - c2z)   r1^2 - r2^2 + c2x^2 + c2y^2 + c2z^2 - c1x^2 - c1y^2 - c1z^2][x'] = 0
        // [2*(c1x - c3x)   2*(c1y - c3y)   2*(c1z - c3z)   r1^2 - r3^2 + c3x^2 + c3y^3 + c3z^2 - c1x^2 - c1y^2 - c1z^2][y'] = 0
        // [2*(c1x - c4x)   2*(c1y - c4y)   2*(c1z - c4z)   r1^2 - r4^2 + c4x^2 + c4y^2 + c4z^2 - c1x^2 - c1y^2 - c1z^2][z'] = 0
        //                                                                                                              [w']

        // This can be solved by using the SVD decomposition of matrix A and picking the last column of
        // resulting V matrix. At least 3 equations are required to find a solution, since 1 additional
        // point is used as a reference, at least 4 points are required.

        if (!isReady()) {
            throw new NotReadyException();
        }
        if (isLocked()) {
            throw new LockedException();
        }

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onSolveStart(this);
            }

            final int numberOfPositions = mPositions.length;
            final int numberOfPositionsMinus1 = numberOfPositions - 1;
            final int dims = getNumberOfDimensions();

            final double referenceDistance = mDistances[0];
File Line
com/irurueta/navigation/lateration/PROSACRobustLateration2DSolver.java 483
com/irurueta/navigation/lateration/RANSACRobustLateration2DSolver.java 238
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Solves the lateration problem.
     *
     * @return estimated position.
     * @throws LockedException          if instance is busy solving the lateration problem.
     * @throws NotReadyException        is solver is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public Point2D solve() throws LockedException, NotReadyException,
            RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Point2D> innerEstimator =
File Line
com/irurueta/navigation/lateration/PROSACRobustLateration3DSolver.java 482
com/irurueta/navigation/lateration/RANSACRobustLateration3DSolver.java 238
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Solves the lateration problem.
     *
     * @return estimated position.
     * @throws LockedException          if instance is busy solving the lateration problem.
     * @throws NotReadyException        is solver is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public Point3D solve() throws LockedException, NotReadyException,
            RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Point3D> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1752
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1771
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 840
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 851
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        mGravityNorm = computeGravityNorm();

        final PROSACRobustEstimator<PreliminaryResult> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1901
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 574
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1925
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 989
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 368
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 1000
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1572
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1804
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1542
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 4067
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 581
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 4132
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 843
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 912
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 828
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 2036
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 375
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 2066
            mInliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2800
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2957
            final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3461
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3619
            final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 6262
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 6527
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 12017
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 12609
            result.mEstimatedMa = mInnerCalibrator.getEstimatedMa();

            solutions.add(result);

        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (mRefineResult && mInliersData != null) {
            final BitSet inliers = mInliersData.getInliers();
            final int nSamples = mMeasurements.size();

            final List<StandardDeviationBodyKinematics> inlierMeasurements =
                    new ArrayList<>();
            for (int i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(mMeasurements.get(i));
                }
            }

            try {
                mInnerCalibrator.setBias(mBiasX, mBiasY, mBiasZ);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2978
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3138
            final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3649
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3809
            final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3348
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3283
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4774
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4836
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        final Matrix g = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        g.setElementAtIndex(0, g11);
        g.setElementAtIndex(1, g21);
        g.setElementAtIndex(2, g31);

        g.setElementAtIndex(3, g12);
        g.setElementAtIndex(4, g22);
        g.setElementAtIndex(5, g32);

        g.setElementAtIndex(6, g13);
        g.setElementAtIndex(7, g23);
        g.setElementAtIndex(8, g33);

        setResult(m, b, g);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3519
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3125
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4586
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5040
        m.setElementAtIndex(5, m32);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        final Matrix g = new Matrix(BodyKinematics.COMPONENTS,
                BodyKinematics.COMPONENTS);
        g.setElementAtIndex(0, g11);
        g.setElementAtIndex(1, g21);
        g.setElementAtIndex(2, g31);

        g.setElementAtIndex(3, g12);
        g.setElementAtIndex(4, g22);
        g.setElementAtIndex(5, g32);

        g.setElementAtIndex(6, g13);
        g.setElementAtIndex(7, g23);
        g.setElementAtIndex(8, g33);

        setResult(m, b, g);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5092
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5160
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5402
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5470
    private void setInputDataWithGDependentCrossBiases()
            throws AlgebraException,
            InvalidSourceAndDestinationFrameTypeException {
        // compute reference frame at current position
        final NEDPosition nedPosition = getNedPosition();
        final CoordinateTransformation nedC = new CoordinateTransformation(
                FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
        final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
                .convertNEDtoECEFAndReturnNew(nedFrame);
        final BodyKinematics refKinematics = ECEFKinematicsEstimator
                .estimateKinematicsAndReturnNew(mTimeInterval, ecefFrame,
                        ecefFrame);

        final double refAngularRateX = refKinematics.getAngularRateX();
        final double refAngularRateY = refKinematics.getAngularRateY();
        final double refAngularRateZ = refKinematics.getAngularRateZ();

        final double w2 = mTurntableRotationRate * mTurntableRotationRate;

        final int numMeasurements = mMeasurements.size();
        final Matrix x = new Matrix(numMeasurements, 2 * BodyKinematics.COMPONENTS);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3820
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4173
                result[2] = bz + omegatruez + sz * omegatruez
                        + g31 * ftruex + g32 * ftruey + g33 * ftruez;

                jacobian.setElementAt(0, 0, 1.0);
                jacobian.setElementAt(0, 1, 0.0);
                jacobian.setElementAt(0, 2, 0.0);
                jacobian.setElementAt(0, 3, omegatruex);
                jacobian.setElementAt(0, 4, 0.0);
                jacobian.setElementAt(0, 5, 0.0);
                jacobian.setElementAt(0, 6, omegatruey);
                jacobian.setElementAt(0, 7, omegatruez);
                jacobian.setElementAt(0, 8, 0.0);
                jacobian.setElementAt(0, 9, ftruex);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4014
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5100
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3808
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4894
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3006
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3183
            final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3745
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3922
            final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3115
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3292
            final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3854
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4031
            final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
File Line
com/irurueta/navigation/indoor/Utils.java 354
com/irurueta/navigation/indoor/Utils.java 605
                    y[0] = fingerprintRssi
                            - 10.0 * pathLossExponent * crossDiff / (ln10 * d1a2);

                    //compute gradient (is a jacobian having 1 row and 8 columns)


                    //derivative of rssi respect to fingerprint rssi
                    final double derivativeFingerprintRssi = 1.0;

                    //derivative of rssi respect to path-loss exponent

                    //diff(Pr(pi))/diff(n) = -10*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
                    //  -10*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
                    final double derivativePathLossExponent = -10.0 * crossDiff /
                            (ln10 * d1a2);


                    //derivative of rssi respect to x1

                    //We have
                    //Pr(pi) = Pr(p1) -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/
                    //      (ln(10)*((x1 - xa)^2 + (y1 - ya)^2))

                    //and we know that: (f(x)/g(x))' = (f'(x)*g(x) - f(x)*g'(x))/g(x)^2
                    //and also that (f(x)*g(x))' = f'(x)*g(x) + f(x)*g'(x)

                    //Hence
                    //diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(x1) =
                    //  diff(x1*xi -xa*xi -x1^2 + xa*x1)/diff(x1) =
                    //  diff(-x1^2 + (xi + xa)*x1 - xa*xi)/diff(x1) =
                    //  -2*x1 + xi + xa

                    //diff(Pr(pi))/diff(x1) = -10*n/ln(10)*((-2*x1 + xi + xa)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^2
                    //diff(Pr(pi))/diff(x1) = -10*n/ln(10)*((-2*x1 + xi + xa)*d1a^2 - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(x1 - xa))/d1a^4
                    final double tmpX = 2.0 * crossDiff * diffX1a;
                    final double derivativeX1 = -10.0 * pathLossExponent / ln10 * ((-2.0 * x1 + xi + xa) * d1a2
                            - tmpX) / d1a4;

                    //derivative of rssi respect to y1

                    //diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(y1) =
                    //  diff(y1*yi -ya*yi -y1^2 + ya*y1)/diff(y1) =
                    //  diff(-y1^2 + (yi + ya)*y1 - ya*yi)/diff(y1) =
                    //  -2*y1 + yi + ya

                    //diff(Pr(pi))/diff(y1) = -10*n/ln(10)*((-2*y1 + yi + ya)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2)^2
                    //diff(Pr(pi))/diff(y1) = -10*n/ln(10)*((-2*y1 + yi + ya)*d1a^2 - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(y1 - ya))/d1a^4
                    final double tmpY = 2.0 * crossDiff * diffY1a;
                    final double derivativeY1 = -10.0 * pathLossExponent / ln10 * ((-2.0 * y1 + yi + ya) * d1a2
                            - tmpY) / d1a4;


                    //derivative of rssi respect to xa

                    //diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(xa) =
                    //  diff(x1*xi -xa*xi -x1^2 + xa*x1)/diff(xa) =
                    //  x1 - xi

                    //diff(Pr(pi))/diff(xa) = -10*n/ln(10)*((x1 - xi)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*-2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^2
                    //diff(Pr(pi))/diff(xa) = -10*n/ln(10)*(-(xi - x1)*d1a^2 + ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(x1 - xa))/d1a^4
                    final double derivativeXa = -10.0 * pathLossExponent / ln10 * (-diffXi1 * d1a2
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingAndRssiRadioSourceEstimator2D.java 400
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator2D.java 410
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<RobustRangingAndRssiRadioSourceEstimator.Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingAndRssiRadioSourceEstimator2D.java 400
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingAndRssiRadioSourceEstimator3D.java 399
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<RobustRangingAndRssiRadioSourceEstimator.Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingAndRssiRadioSourceEstimator3D.java 399
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator3D.java 410
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<RobustRangingAndRssiRadioSourceEstimator.Solution<Point3D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator2D.java 1049
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator2D.java 543
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator2D.java 1060
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 1075
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 566
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 1095
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 591
            innerEstimator.setUseInlierThresholds(false);

            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point2D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
File Line
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator3D.java 1047
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator3D.java 542
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator3D.java 1061
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 565
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 1092
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator3D.java 355
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 598
            innerEstimator.setUseInlierThresholds(false);

            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point3D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMedS;
File Line
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1033
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1125
                result.getBuffer());
    }

    /**
     * Fixes provided measured angular rate values by undoing the errors
     * introduced by the gyroscope model to restore the true angular
     * rate.
     *
     * @param measuredAngularRateX x-coordinate of measured angular rate
     *                             expressed in radians per second (rad/s).
     * @param measuredAngularRateY y-coordinate of measured angular rate
     *                             expressed in radians per second (rad/s).
     * @param measuredAngularRateZ z-coordinate of measured angular rate
     *                             expressed in radians per second (rad/s).
     * @param trueFx               x-coordinate of true (i.e. fixed)
     *                             specific force expressed in meters per
     *                             squared second (m/s^2).
     * @param trueFy               y-coordinate of true (i.e. fixed)
     *                             specific force expressed in meters per
     *                             squared second (m/s^2).
     * @param trueFz               z-coordinate of true (i.e. fixed)
     *                             specific force expressed in meters per
     *                             squared second (m/s^2).
     * @param biasX                x-coordinate of bias expressed in
     *                             meters per squared second (m/s^2).
     * @param biasY                y-coordinate of bias expressed in
     *                             meters per squared second (m/s^2).
     * @param biasZ                z-coordinate of bias expressed in
     *                             meters per squared second (m/s^2).
     * @param sx                   initial x scaling factor.
     * @param sy                   initial y scaling factor.
     * @param sz                   initial z scaling factor.
     * @param mxy                  initial x-y cross coupling error.
     * @param mxz                  initial x-z cross coupling error.
     * @param myx                  initial y-x cross coupling error.
     * @param myz                  initial y-z cross coupling error.
     * @param mzx                  initial z-x cross coupling error.
     * @param mzy                  initial z-y cross coupling error.
     * @param g11                  element 1,1 of g-dependant cross biases.
     * @param g21                  element 2,1 of g-dependant cross biases.
     * @param g31                  element 3,1 of g-dependant cross biases.
     * @param g12                  element 1,2 of g-dependant cross biases.
     * @param g22                  element 2,2 of g-dependant cross biases.
     * @param g32                  element 3,2 of g-dependant cross biases.
     * @param g13                  element 1,3 of g-dependant cross biases.
     * @param g23                  element 2,3 of g-dependant cross biases.
     * @param g33                  element 3,3 of g-dependant cross biases.
     * @param result               instance where restored true angular rate
     *                             will be stored. Must have length 3.
     * @throws AlgebraException         if there are numerical instabilities.
     * @throws IllegalArgumentException if any of the provided parameters
     *                                  does not have proper size.
     */
    public void fix(
            final double measuredAngularRateX,
            final double measuredAngularRateY,
            final double measuredAngularRateZ,
            final double trueFx,
            final double trueFy,
            final double trueFz,
            final double biasX, final double biasY, final double biasZ,
            final double sx, final double sy, final double sz,
            final double mxy, final double mxz,
            final double myx, final double myz,
            final double mzx, final double mzy,
            final double g11, final double g21, final double g31,
            final double g12, final double g22, final double g32,
            final double g13, final double g23, final double g33,
            final double[] result) throws AlgebraException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7551
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3795
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4993
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5350
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2540
        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];
        final double m21 = result[4];
        final double m31 = result[5];

        final double m12 = result[6];
        final double m22 = result[7];
        final double m32 = result[8];

        final double m13 = result[9];
        final double m23 = result[10];
        final double m33 = result[11];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 61
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 60
public abstract class RobustKnownBiasAndFrameAccelerometerCalibrator {

    /**
     * Indicates whether by default a common z-axis is assumed for both the accelerometer
     * and gyroscope.
     */
    public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;

    /**
     * Required minimum number of measurements.
     */
    public static final int MINIMUM_MEASUREMENTS = 4;

    /**
     * Indicates that by default a linear calibrator is used for preliminary solution estimation.
     * The result obtained on each preliminary solution might be later refined.
     */
    public static final boolean DEFAULT_USE_LINEAR_CALIBRATOR = true;

    /**
     * Indicates that by default preliminary solutions are refined.
     */
    public static final boolean DEFAULT_REFINE_PRELIMINARY_SOLUTIONS = false;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD =
            RobustEstimatorMethod.LMedS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen subsamples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Contains a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     */
    protected List<StandardDeviationFrameBodyKinematics> mMeasurements;

    /**
     * Listener to be notified of events such as when calibration starts, ends or its
     * progress significantly changes.
     */
    protected RobustKnownBiasAndFrameAccelerometerCalibratorListener mListener;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 6024
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 6140
        mBiasZ = convertAcceleration(biasZ);
    }

    /**
     * Internally sets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known accelerometer bias.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    private void internalSetBias(final double[] bias) {
        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        mBiasX = bias[0];
        mBiasY = bias[1];
        mBiasZ = bias[2];
    }

    /**
     * Internally sets known accelerometer bias as a column matrix.
     * Matrix values are expressed in meters per squared second (m/s^2).
     *
     * @param bias accelerometer bias to be set.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    private void internalSetBias(final Matrix bias) {
        if (bias.getRows() != BodyKinematics.COMPONENTS
                || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        mBiasX = bias.getElementAtIndex(0);
        mBiasY = bias.getElementAtIndex(1);
        mBiasZ = bias.getElementAtIndex(2);
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3831
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3869
                jacobian.setElementAt(2, 8, omegatruey);
                jacobian.setElementAt(2, 9, 0.0);
                jacobian.setElementAt(2, 10, 0.0);
                jacobian.setElementAt(2, 11, 0.0);
                jacobian.setElementAt(2, 12, 0.0);
                jacobian.setElementAt(2, 13, 0.0);
                jacobian.setElementAt(2, 14, 0.0);
                jacobian.setElementAt(2, 15, ftruex);
                jacobian.setElementAt(2, 16, ftruey);
                jacobian.setElementAt(2, 17, ftruez);
            }
        });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double sx = result[0];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3404
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3500
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences   collection of sequences containing timestamped body
     *                    kinematics measurements.
     * @param initialBias initial gyroscope bias to be used to find a solution.
     *                    This must be 3x1 and is expressed in radians per
     *                    second (rad/s).
     * @param initialMg   initial gyroscope scale factors and cross coupling
     *                    errors matrix. Must be 3x3.
     * @param initialGg   initial gyroscope G-dependent cross biases
     *                    introduced on the gyroscope by the specific forces
     *                    sensed by the accelerometer. Must be 3x3.
     * @param listener    listener to handle events raised by this
     *                    calibrator.
     * @param method      robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3198
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3294
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences   collection of sequences containing timestamped body
     *                    kinematics measurements.
     * @param initialBias initial gyroscope bias to be used to find a solution.
     *                    This must be 3x1 and is expressed in radians per
     *                    second (rad/s).
     * @param initialMg   initial gyroscope scale factors and cross coupling
     *                    errors matrix. Must be 3x3.
     * @param initialGg   initial gyroscope G-dependent cross biases
     *                    introduced on the gyroscope by the specific forces
     *                    sensed by the accelerometer. Must be 3x3.
     * @param listener    listener to handle events raised by this
     *                    calibrator.
     * @param method      robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 564
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1755
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException,
            CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    @Override
    public double[] getEstimatedHardIron() {
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2319
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1306
                                          final ECEFFrame frame,
                                          final CoordinateTransformation oldC,
                                          final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                          final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC,
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param frame        body ECEF frame containing current position, velocity and
     *                     body-to-ECEF frame coordinate transformation.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2348
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1277
                                          final ECEFFrame frame,
                                          final CoordinateTransformation oldC,
                                          final Speed oldVx, final Speed oldVy, final Speed oldVz,
                                          final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC,
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along EVEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final double timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9846
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9900
    public static void navigateECEF(final double timeInterval,
                                    final double oldX,
                                    final double oldY,
                                    final double oldZ,
                                    final CoordinateTransformation oldC,
                                    final ECEFVelocity oldVelocity,
                                    final Acceleration fx,
                                    final Acceleration fy,
                                    final Acceleration fz,
                                    final double angularRateX,
                                    final double angularRateY,
                                    final double angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                convertAccelerationToDouble(fx), convertAccelerationToDouble(fy),
                convertAccelerationToDouble(fz), angularRateX, angularRateY,
                angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10267
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10321
    public static void navigateECEF(final double timeInterval,
                                    final double oldX,
                                    final double oldY,
                                    final double oldZ,
                                    final CoordinateTransformation oldC,
                                    final ECEFVelocity oldVelocity,
                                    final double fx,
                                    final double fy,
                                    final double fz,
                                    final AngularSpeed angularRateX,
                                    final AngularSpeed angularRateY,
                                    final AngularSpeed angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                fx, fy, fz, convertAngularSpeedToDouble(angularRateX),
                convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/indoor/position/PROMedSRobustMixedPositionEstimator2D.java 367
com/irurueta/navigation/indoor/position/PROMedSRobustMixedPositionEstimator3D.java 367
com/irurueta/navigation/indoor/position/PROMedSRobustRangingAndRssiPositionEstimator2D.java 367
com/irurueta/navigation/indoor/position/PROMedSRobustRangingAndRssiPositionEstimator3D.java 367
com/irurueta/navigation/indoor/position/PROMedSRobustRangingPositionEstimator2D.java 366
com/irurueta/navigation/indoor/position/PROMedSRobustRangingPositionEstimator3D.java 366
com/irurueta/navigation/indoor/position/PROMedSRobustRssiPositionEstimator2D.java 365
            final RobustMixedPositionEstimatorListener<Point2D> listener) {
        this(sources, fingerprint, listener);
        internalSetSourceQualityScores(sourceQualityScores);
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Returns quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each radio source.
     */
    public double[] getSourceQualityScores() {
        return mSourceQualityScores;
    }

    /**
     * Sets quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the radio source.
     *
     * @param sourceQualityScores quality scores corresponding to each radio source.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setSourceQualityScores(final double[] sourceQualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSourceQualityScores(sourceQualityScores);
    }

    /**
     * Gets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each reading within provided
     * fingerprint.
     */
    public double[] getFingerprintReadingsQualityScores() {
        return mFingerprintReadingsQualityScores;
    }

    /**
     * Sets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @param fingerprintReadingsQualityScores quality scores corresponding to each
     *                                         reading within provided fingerprint.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return ((PROMedSRobustLateration2DSolver) mLaterationSolver).
File Line
com/irurueta/navigation/indoor/position/PROSACRobustMixedPositionEstimator2D.java 365
com/irurueta/navigation/indoor/position/PROSACRobustMixedPositionEstimator3D.java 366
com/irurueta/navigation/indoor/position/PROSACRobustRangingAndRssiPositionEstimator2D.java 370
com/irurueta/navigation/indoor/position/PROSACRobustRangingAndRssiPositionEstimator3D.java 370
com/irurueta/navigation/indoor/position/PROSACRobustRangingPositionEstimator2D.java 366
com/irurueta/navigation/indoor/position/PROSACRobustRangingPositionEstimator3D.java 366
com/irurueta/navigation/indoor/position/PROSACRobustRssiPositionEstimator2D.java 368
com/irurueta/navigation/indoor/position/PROSACRobustRssiPositionEstimator3D.java 368
            final RobustMixedPositionEstimatorListener<Point2D> listener) {
        this(sources, fingerprint, listener);
        internalSetSourceQualityScores(sourceQualityScores);
        internalSetFingerprintReadingsQualityScores(fingerprintReadingQualityScores);
    }

    /**
     * Returns quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each radio source.
     */
    public double[] getSourceQualityScores() {
        return mSourceQualityScores;
    }

    /**
     * Sets quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the radio source.
     *
     * @param sourceQualityScores quality scores corresponding to each radio source.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setSourceQualityScores(final double[] sourceQualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSourceQualityScores(sourceQualityScores);
    }

    /**
     * Gets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each reading within provided
     * fingerprint.
     */
    public double[] getFingerprintReadingsQualityScores() {
        return mFingerprintReadingsQualityScores;
    }

    /**
     * Sets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @param fingerprintReadingsQualityScores quality scores corresponding to each
     *                                         reading within provided fingerprint.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return ((PROSACRobustLateration2DSolver) mLaterationSolver).
File Line
com/irurueta/navigation/indoor/position/SequentialRobustMixedPositionEstimator.java 1511
com/irurueta/navigation/indoor/position/SequentialRobustRangingAndRssiPositionEstimator.java 1486
            final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }

        internalSetFingerprint(fingerprint);
    }

    /**
     * Returns quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the radio source.
     *
     * @return quality scores corresponding to each radio source.
     */
    public double[] getSourceQualityScores() {
        return mSourceQualityScores;
    }

    /**
     * Sets quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the radio source.
     *
     * @param sourceQualityScores quality scores corresponding to each radio source.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setSourceQualityScores(final double[] sourceQualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSourceQualityScores(sourceQualityScores);
    }

    /**
     * Gets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     *
     * @return quality scores corresponding to each reading within provided fingerprint.
     */
    public double[] getFingerprintReadingsQualityScores() {
        return mFingerprintReadingsQualityScores;
    }

    /**
     * Sets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     *
     * @param fingerprintReadingsQualityScores quality scores corresponding to each
     *                                         reading within provided fingerprint.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Gets listener to be notified of events raised by this instance.
     *
     * @return listener to be notified of events raised by this instance.
     */
    public SequentialRobustMixedPositionEstimatorListener<P> getListener() {
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingRadioSourceEstimator2D.java 322
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingRadioSourceEstimator2D.java 281
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator2D.java 516
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator2D.java 1051
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator2D.java 545
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator2D.java 1062
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 1076
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 567
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 1096
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 592
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point2D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingRadioSourceEstimator3D.java 323
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator3D.java 502
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator3D.java 518
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator3D.java 1049
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator3D.java 544
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator3D.java 1063
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 566
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 1093
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator3D.java 356
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 599
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Solution<Point3D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/indoor/radiosource/RssiRadioSourceEstimator.java 1392
com/irurueta/navigation/indoor/radiosource/RssiRadioSourceEstimator.java 1520
            }
        });

        final int numReadings = mReadings.size();
        try {
            final Matrix x = new Matrix(numReadings, dimsPlus1);
            final double[] y = new double[numReadings];
            final double[] standardDeviations = new double[numReadings];
            for (int i = 0; i < numReadings; i++) {
                reading = mReadings.get(i);
                final P position = reading.getPosition();

                for (int j = 0; j < dims; j++) {
                    x.setElementAt(i, j, position.getInhomogeneousCoordinate(j));
                }
                x.setElementAt(i, dims, initialTransmittedPowerdBm);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1352
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 873
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1390
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 931
        fillMa(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException if there are numerical errors.
     */
    private void calibrateGeneral() throws AlgebraException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // Hence:
        //  [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        //  [fmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [ftruey]
        //  [fmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [ftruez]

        //  [fmeasx] = [bx] +   [1+sx   mxy     mxz ][ftruex]
        //  [fmeasy]   [by]     [myx    1+sy    myz ][ftruey]
        //  [fmeasz]   [bz]     [mzx    mzy     1+sz][ftruez]

        //  fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + myx * ftruex + (1+sy) * ftruey + myz * ftruez
        //  fmeasz = bz + mzx * ftruex + mzy * ftruey + (1+sz) * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
        // Reordering:
        //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
        //  fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

        //  fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy - ftruey - by = myx * ftruex + sy * ftruey + myz * ftruez
        //  fmeasz - ftruez - bz = mzx * ftruex + mzy * ftruey + sz * ftruez

        // [ftruex  0       0       ftruey  ftruez  0       0       0       0     ][sx ] = [fmeasx - ftruex - bx]
        // [0       ftruey  0       0       0       ftruex  ftruez  0       0     ][sy ]   [fmeasy - ftruey - by]
        // [0       0       ftruez  0       0       0       0       ftruex  ftruey][sz ]   [fmeasz - ftruez - bz]
        //                                                                         [mxy]
        //                                                                         [mxz]
        //                                                                         [myx]
        //                                                                         [myz]
        //                                                                         [mzx]
        //                                                                         [mzy]

        final BodyKinematics expectedKinematics = new BodyKinematics();

        final int rows = EQUATIONS_PER_MEASUREMENT * mMeasurements.size();
        final Matrix a = new Matrix(rows, GENERAL_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double fMeasX = measuredKinematics.getFx();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1751
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1770
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1419
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1387
                mQualityScores.length == mMeasurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3925
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4281
            final double biasX, final double biasY, final double biasZ,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param measurements  list of body kinematics measurements with standard
     *                      deviations taken at different frames (positions, orientations
     *                      and velocities).
     * @param biasX         known x coordinate of accelerometer bias expressed in meters per
     *                      squared second (m/s^2).
     * @param biasY         known y coordinate of accelerometer bias expressed in meters per
     *                      squared second (m/s^2).
     * @param biasZ         known z coordinate of accelerometer bias expressed in meters per
     *                      squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores,
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2798
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3459
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2839
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3500
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and
     *                       is expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2976
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3647
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3018
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3689
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1573
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1543
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 844
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 829
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 582
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2286
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 373
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1136
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3775
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4825
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3892
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4961
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3999
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4360
            final double biasX, final double biasY, final double biasZ,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, measurements, biasX, biasY, biasZ);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param measurements  list of body kinematics measurements with standard
     *                      deviations taken at different frames (positions, orientations
     *                      and velocities).
     * @param biasX         known x coordinate of gyroscope bias expressed in radians per
     *                      second (rad/s).
     * @param biasY         known y coordinate of gyroscope bias expressed in radians per
     *                      second (rad/s).
     * @param biasZ         known z coordinate of gyroscope bias expressed in radians per
     *                      second (rad/s).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3569
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4619
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3686
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4755
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3004
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3743
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3048
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3787
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3113
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3852
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3157
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3896
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/lateration/LMedSRobustLateration2DSolver.java 345
com/irurueta/navigation/lateration/MSACRobustLateration2DSolver.java 300
com/irurueta/navigation/lateration/PROSACRobustLateration2DSolver.java 632
com/irurueta/navigation/lateration/RANSACRobustLateration2DSolver.java 382
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            Point2D result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (mListener != null) {
                mListener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/lateration/LMedSRobustLateration3DSolver.java 347
com/irurueta/navigation/lateration/MSACRobustLateration3DSolver.java 301
com/irurueta/navigation/lateration/PROSACRobustLateration3DSolver.java 630
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            Point3D result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (mListener != null) {
                mListener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/geodesic/PolygonArea.java 280
com/irurueta/navigation/geodesic/PolygonArea.java 348
        if ((crossings & 1) != 0) {
            tempsum += (tempsum < 0 ? 1 : -1) * mArea0 / 2;
        }

        //area is with the clockwise sense. If !reverse convert to counter-clockwise convention
        if (!reverse) {
            tempsum *= -1;
        }

        //if sign put area in (-area0/2, area0/2], else put area in [0, area0)
        if (sign) {
            if (tempsum > mArea0 / 2) {
                tempsum -= mArea0;
            } else if (tempsum <= -mArea0 / 2) {
                tempsum += mArea0;
            }
        } else {
            if (tempsum >= mArea0) {
                tempsum -= mArea0;
            } else if (tempsum < 0) {
                tempsum += mArea0;
            }
        }
        return new PolygonResult(num, perimeter, 0 + tempsum);
    }

    /**
     * Return the results assuming a tentative final test point is added via an azimuth and distance;
     * however, the data for the test point is not saved.
     * This lets you report a running result for the perimeter and area as the user moves the mouse
     * cursor. Ordinary floating point arithmetic is used to accumulate the data for the test point;
     * thus the area and perimeter returned are less accurate than if addPoint and compute are used.
     *
     * @param azi     azimuth at current point (degrees).
     * @param s       distance from current point to final test point (meters).
     * @param reverse if true then clockwise (instead of counter-clockwise) traversal counts as a
     *                positive area.
     * @param sign    if true then return a signed result for the area if the polygon is traversed in
     *                the "wrong" direction instead of returning the area for the rest of the earth.
     * @return PolygonResult(<i>num</i>, <i>perimeter</i>, <i>area</i>) where <i>num</i> is the
     * number of vertices, <i>perimeter</i> is the perimeter of the polygon or the length of the
     * polyline (meters), and <i>area</i> is the area of the polygon (meters<sup>2</sup>) or
     * Double.NaN of <i>polyline</i> is true in the constructor.
     */
    public PolygonResult testEdge(
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator2D.java 410
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator3D.java 410
            final RobustRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator2D.java 560
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator2D.java 502
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator2D.java 356
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator2D.java 603
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            Solution<Point2D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator3D.java 558
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingRadioSourceEstimator3D.java 279
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 1081
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator3D.java 592
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            Solution<Point3D> result = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();
            attemptRefine(result);

            if (mListener != null) {
                mListener.onEstimateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            mLocked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator2D.java 375
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator3D.java 375
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator2D.java 387
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator3D.java 389
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if robust estimator is locked because an
     *                                  estimation is already in progress.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/RangingAndRssiRadioSourceEstimator.java 1068
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator.java 2322
    private RangingReadingLocated<S, P> createRangingReading(
            final RangingAndRssiReadingLocated<S, P> reading) {
        return new RangingReadingLocated<>(reading.getSource(),
                reading.getDistance(), reading.getPosition(),
                reading.getDistanceStandardDeviation(),
                reading.getPositionCovariance());
    }

    /**
     * Creates an RSSI reading from a ranging and RSSI reading.
     *
     * @param reading input reading to convert from.
     * @return an RSSI reading containing only the RSSI data of input reading.
     */
    private RssiReadingLocated<S, P> createRssiReading(
            final RangingAndRssiReadingLocated<S, P> reading) {
        return new RssiReadingLocated<>(reading.getSource(),
                reading.getRssi(), reading.getPosition(),
                reading.getRssiStandardDeviation(),
                reading.getPositionCovariance());
    }
}
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator3D.java 2897
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator3D.java 2876
com/irurueta/navigation/indoor/radiosource/RssiRadioSourceEstimator3D.java 469
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator3D.java 905
                    source.getFrequency(), accessPoint.getSsid(),
                    getEstimatedTransmittedPowerdBm(),
                    transmittedPowerStandardDeviation,
                    getEstimatedPathLossExponent(),
                    pathlossExponentStandardDeviation,
                    estimatedPosition,
                    estimatedPositionCovariance);
        } else if (source instanceof Beacon) {
            final Beacon beacon = (Beacon) source;
            return new BeaconWithPowerAndLocated3D(beacon.getIdentifiers(),
                    getEstimatedTransmittedPowerdBm(), beacon.getFrequency(),
                    beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(),
                    beacon.getManufacturer(), beacon.getServiceUuid(),
                    beacon.getBluetoothName(),
                    getEstimatedPathLossExponent(),
                    transmittedPowerStandardDeviation,
                    pathlossExponentStandardDeviation,
                    estimatedPosition, estimatedPositionCovariance);
        } else {
            return null;
        }
    }
File Line
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1612
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1685
    public double[] fixAndReturnNew(
            final double measuredAngularRateX,
            final double measuredAngularRateY,
            final double measuredAngularRateZ,
            final double trueFx,
            final double trueFy,
            final double trueFz,
            final double biasX, final double biasY, final double biasZ,
            final double sx, final double sy, final double sz,
            final double mxy, final double mxz,
            final double myx, final double myz,
            final double mzx, final double mzy,
            final double g11, final double g21, final double g31,
            final double g12, final double g22, final double g32,
            final double g13, final double g23, final double g33)
            throws AlgebraException {

        final double[] result = new double[BodyKinematics.COMPONENTS];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1237
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 760
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1254
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 785
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws AlgebraException if there are numerical errors.
     */
    private void calibrateCommonAxis() throws AlgebraException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // Hence:
        //  [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        //  [fmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [ftruey]
        //  [fmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [ftruez]

        // where myx = mzx = mzy = 0

        // Hence:
        //  [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        //  [fmeasy] = [by]     [0  1   0]   [0     sy  myz]    [ftruey]
        //  [fmeasz] = [bz]     [0  0   1]   [0     0   sz ]    [ftruez]

        //  [fmeasx] = [bx] +   [1+sx   mxy     mxz ][ftruex]
        //  [fmeasy]   [by]     [0      1+sy    myz ][ftruey]
        //  [fmeasz]   [bz]     [0      0       1+sz][ftruez]

        //  fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + (1+sy) * ftruey + myz * ftruez
        //  fmeasz = bz + (1+sz) * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myz
        // Reordering:
        //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy = by + ftruey + sy * ftruey + myz * ftruez
        //  fmeasz = bz + ftruez + sz * ftruez

        //  fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
        //  fmeasy - ftruey - by = sy * ftruey + myz * ftruez
        //  fmeasz - ftruez - bz = sz * ftruez

        // [ftruex  0       0       ftruey  ftruez  0     ][sx ] = [fmeasx - ftruex - bx]
        // [0       ftruey  0       0       0       ftruez][sy ]   [fmeasy - ftruey - by]
        // [0       0       ftruez  0       0       0     ][sz ]   [fmeasz - ftruez - bz]
        //                                                 [mxy]
        //                                                 [mxz]
        //                                                 [myz]

        final BodyKinematics expectedKinematics = new BodyKinematics();

        final int rows = EQUATIONS_PER_MEASUREMENT * mMeasurements.size();
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final Matrix b = new Matrix(rows, 1);
        int i = 0;
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double fMeasX = measuredKinematics.getFx();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1798
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 902
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 583
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2288
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 373
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1134
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1902
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 575
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1926
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 990
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 369
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 1001
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1805
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 4068
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 582
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 4133
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 913
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 2037
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 376
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 2067
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 582
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2286
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 373
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1136
            innerEstimator.setComputeAndKeepInliersEnabled(
                    mComputeAndKeepInliers || mRefineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(
                    mComputeAndKeepResiduals || mRefineResult);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4794
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5060
        setResult(m, g);
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope and G-dependent cross biases are ignored.
     *
     * @throws AlgebraException                              if there are numerical errors.
     * @throws FittingException                              if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException      if fitter is not ready.
     * @throws InvalidSourceAndDestinationFrameTypeException never happens.
     */
    private void calibrateCommonAxis()
            throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException,
            InvalidSourceAndDestinationFrameTypeException {

        // The gyroscope model is
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Since G-dependent cross giases are ignored, we can assume that Gg = 0

        // Hence:
        // Ωmeas = bg + (I + Mg) * Ωtrue

        // For convergence purposes of the Levenberg-Marquardt algorithm, the
        // gyroscope model can be better expressed as:
        // Ωmeas = T*K*(Ωtrue + b)
        // Ωmeas = M*(Ωtrue + b)
        // Ωmeas = M*Ωtrue + M*b

        // where:
        // M = I + Mg
        // bg = M*b = (I + Mg)*b --> b = M^-1*bg

        // We know that the norm of the true angular rate when the device is in a pixed
        // and unknown position and orientation is equal to the Earth rotation rate.
        // ||Ωtrue|| = 7.292115E-5 rad/s

        // Hence
        // Ωmeas - M*b = M*Ωtrue

        // M^-1 * (Ωmeas - M*b) = Ωtrue

        // ||Ωtrue||^2 = (M^-1 * (Ωmeas - M*b))^T*(M^-1 * (Ωmeas - M*b))
        // ||Ωtrue||^2 = (Ωmeas - M*b)^T * (M^-1)^T * M^-1 * (Ωmeas - M*b)
        // ||Ωtrue||^2 = (Ωmeas - M*b)^T * ||M^-1||^2 * (Ωmeas - M*b)
        // ||Ωtrue||^2 = ||Ωmeas - M*b||^2 * ||M^-1||^2

        // Where:

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11 	m12 	m13]
        //     [0 		m22 	m23]
        //     [0 	 	0 		m33]

        final GradientEstimator gradientEstimator = new GradientEstimator(
                new MultiDimensionFunctionEvaluatorListener() {
                    @Override
                    public double evaluate(final double[] point)
                            throws EvaluationException {
                        return evaluateCommonAxis(point);
                    }
                });

        final Matrix initialM = Matrix.identity(
                BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        initialM.add(getInitialMg());

        // Force initial M to be upper diagonal
        initialM.setElementAt(1, 0, 0.0);
        initialM.setElementAt(2, 0, 0.0);
        initialM.setElementAt(2, 1, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 629
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 649
    public MSACRobustEasyGyroscopeCalibrator(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener) {
        super(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, initialBias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 614
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 634
    public MSACRobustKnownBiasEasyGyroscopeCalibrator(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener) {
        super(sequences, commonAxisUsed,
                estimateGDependentCrossBiases, bias, initialMg,
                initialGg, accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 434
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 428
            new EasyGyroscopeCalibrator();

    /**
     * Contains normalized start gravity coordinates.
     * This is reused when computing error residuals.
     */
    private final InhomogeneousPoint3D mStartPoint =
            new InhomogeneousPoint3D();

    /**
     * Contains estimated normalized end gravity coordinates.
     * This is reused when computing error residuals.
     */
    private final InhomogeneousPoint3D mEndPoint =
            new InhomogeneousPoint3D();

    /**
     * Contains expected normalized end gravity coordinates.
     * This is reused when computing error residuals.
     */
    private final InhomogeneousPoint3D mExpectedEndPoint =
            new InhomogeneousPoint3D();

    /**
     * Contains amount of rotation for a given sequence and preliminary
     * solution.
     * This is reused when computing error residuals.
     */
    private final Quaternion mQ = new Quaternion();

    /**
     * Array containing measured specific force coordinates.
     * This is reused when computing error residuals.
     */
    private final double[] mMeasuredSpecificForce = new double[
            BodyKinematics.COMPONENTS];

    /**
     * Array containing fixed specific force coordinates.
     * This is reused when computing error residuals.
     */
    private final double[] mFixedSpecificForce = new double[
            BodyKinematics.COMPONENTS];

    /**
     * Array containing measured angular rate coordinates.
     * This is reused when computing error residuals.
     */
    private final double[] mMeasuredAngularRate = new double[
            BodyKinematics.COMPONENTS];

    /**
     * Array containing fixed angular rate coordinates.
     * This is reused when computing error residuals.
     */
    private final double[] mFixedAngularRate = new double[
            BodyKinematics.COMPONENTS];

    /**
     * An acceleration fixer.
     * This is reused when computing error residuals.
     */
    private final AccelerationFixer mAccelerationFixer =
            new AccelerationFixer();

    /**
     * An angular rate fixer.
     * This is reused when computing error residuals.
     */
    private final AngularRateFixer mAngularRateFixer =
            new AngularRateFixer();

    /**
     * Constructor.
     */
    public RobustEasyGyroscopeCalibrator() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4740
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7424
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6062
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8833
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4966
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7794
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6353
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9284
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2050
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1118
                                          final ECEFFrame frame,
                                          final CoordinateTransformation oldC,
                                          final double oldVx, final double oldVy, final double oldVz,
                                          final BodyKinematics result) {
        estimateKinematics(TimeConverter.convert(timeInterval.getValue().doubleValue(),
                timeInterval.getUnit(), TimeUnit.SECOND),
                frame.getCoordinateTransformation(),
                oldC, frame.getVx(), frame.getVy(), frame.getVz(),
                oldVx, oldVy, oldVz, frame.getX(), frame.getY(), frame.getZ(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param frame        body ECEF frame containing current position, velocity and
     *                     body-to-ECEF frame coordinate transformation.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(final double timeInterval,
                                          final ECEFFrame frame,
File Line
com/irurueta/navigation/lateration/MSACRobustLateration2DSolver.java 232
com/irurueta/navigation/lateration/RANSACRobustLateration2DSolver.java 309
                new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Point2D>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mDistances.length;
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices, final List<Point2D> solutions) {
                        solvePreliminarSolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final Point2D currentEstimation, final int i) {
                        return Math.abs(currentEstimation.distanceTo(mPositions[i]) - mDistances[i]);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustLateration2DSolver.this.isReady();
File Line
com/irurueta/navigation/lateration/MSACRobustLateration3DSolver.java 233
com/irurueta/navigation/lateration/RANSACRobustLateration3DSolver.java 309
                new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Point3D>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mDistances.length;
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices, final List<Point3D> solutions) {
                        solvePreliminarSolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final Point3D currentEstimation, final int i) {
                        return Math.abs(currentEstimation.distanceTo(mPositions[i]) - mDistances[i]);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustLateration3DSolver.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator.java 1145
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator.java 2322
    private RangingReadingLocated<S, P> createRangingReading(
            final RangingAndRssiReadingLocated<S, P> reading) {
        return new RangingReadingLocated<>(reading.getSource(),
                reading.getDistance(), reading.getPosition(),
                reading.getDistanceStandardDeviation(),
                reading.getPositionCovariance());
    }

    /**
     * Creates an RSSI reading from a ranging and RSSI reading.
     *
     * @param reading input reading to convert from.
     * @return an RSSI reading containing only the RSSI data of input reading.
     */
    private RssiReadingLocated<S, P> createRssiReading(
            final RangingAndRssiReadingLocated<S, P> reading) {
        return new RssiReadingLocated<>(reading.getSource(),
                reading.getRssi(), reading.getPosition(),
                reading.getRssiStandardDeviation(),
                reading.getPositionCovariance());
    }
File Line
com/irurueta/navigation/indoor/radiosource/RangingAndRssiRadioSourceEstimator.java 1068
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator.java 2453
    private RangingReadingLocated<S, P> createRangingReading(
            final RangingAndRssiReadingLocated<S, P> reading) {
        return new RangingReadingLocated<>(reading.getSource(),
                reading.getDistance(), reading.getPosition(),
                reading.getDistanceStandardDeviation(),
                reading.getPositionCovariance());
    }

    /**
     * Creates an RSSI reading from a ranging and RSSI reading.
     *
     * @param reading input reading to convert from.
     * @return an RSSI reading containing only the RSSI data of input reading.
     */
    private RssiReadingLocated<S, P> createRssiReading(
            final RangingAndRssiReadingLocated<S, P> reading) {
        return new RssiReadingLocated<>(reading.getSource(),
                reading.getRssi(), reading.getPosition(),
                reading.getRssiStandardDeviation(),
                reading.getPositionCovariance());
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2513
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2780
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2504
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2987
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3499
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3457
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4733
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 5057
            final double[] qualityScores, final double[] bias,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        bias, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        bias, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        bias, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, bias, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, bias, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param bias           known accelerometer bias.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided quality scores length is smaller than
     *                                  4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores,
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double[] bias, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2721
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3382
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias. This must have length 3 and is
     *                     expressed in meters per squared second (m/s^2).
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2841
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2997
            final boolean commonAxisUsed, final double[] bias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and
     *                       is expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2955
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3617
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2995
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3657
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3075
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3737
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final Matrix initialMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, initialMa);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param initialMa    initial scale factors and cross coupling errors matrix.
     * @param listener     listener to handle events raised by this calibrator.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3502
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3659
            final boolean commonAxisUsed, final double[] bias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2897
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3568
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial accelerometer bias to be used to find a solution.
     *                     This must have length 3 and is expressed in meters per
     *                     squared second (m/s^2).
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3020
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3178
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3136
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3807
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3176
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3847
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3256
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3927
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, initialMa);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param initialMa    initial scale factors and cross coupling errors matrix.
     * @param listener     listener to handle events raised by this calibrator.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3691
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3849
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4813
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5135
            final double[] qualityScores, final double[] bias,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(bias,
                        commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(bias,
                        commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(bias,
                        commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, bias, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, bias, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param bias           known gyroscope bias.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided quality scores length is smaller
     *                                  than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double[] bias, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5295
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6617
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8204
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9612
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5466
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6788
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8021
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9429
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5546
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6928
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8612
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10102
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5729
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7111
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8419
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9907
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2920
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3659
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3050
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3227
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3181
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3920
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3225
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3964
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3319
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4058
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron, final Matrix initialMm,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, initialMm);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param initialMm    initial soft-iron matrix containing scale factors
     *                     and cross coupling errors.
     * @param listener     listener to handle events raised by this calibrator.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3789
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3966
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3029
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3768
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3159
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3336
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3290
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4029
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3334
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4073
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3428
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4167
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron, final Matrix initialMm,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, initialMm);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3898
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4075
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10921
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10968
    public static void navigateECEF(final double timeInterval,
                                    final ECEFPosition oldPosition,
                                    final CoordinateTransformation oldC,
                                    final ECEFVelocity oldVelocity,
                                    final Acceleration fx,
                                    final Acceleration fy,
                                    final Acceleration fz,
                                    final AngularSpeed angularRateX,
                                    final AngularSpeed angularRateY,
                                    final AngularSpeed angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                convertAccelerationToDouble(fx),
                convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
                convertAngularSpeedToDouble(angularRateX),
                convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ),
                result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingAndRssiRadioSourceEstimator2D.java 400
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator3D.java 410
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<RobustRangingAndRssiRadioSourceEstimator.Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingAndRssiRadioSourceEstimator3D.java 399
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator2D.java 410
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<RobustRangingAndRssiRadioSourceEstimator.Solution<Point3D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator2D.java 429
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator2D.java 441
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 997
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 488
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 1017
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 513
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator2D.java 277
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator2D.java 524
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point2D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point2D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return MSACRobustRangingAndRssiRadioSourceEstimator2D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator3D.java 429
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator3D.java 443
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 1002
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 487
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 1014
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator3D.java 513
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator3D.java 277
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 522
                            @Override
                            public double getThreshold() {
                                return mThreshold;
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point3D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point3D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return MSACRobustRangingAndRssiRadioSourceEstimator3D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator2D.java 1105
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator2D.java 1932
            final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings,
            final Point2D initialPosition,
            final Double initialTransmittedPowerdBm,
            final double initialPathLossExponent,
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustRangingAndRssiRadioSourceEstimator2D<>(
                        readings, initialPosition, initialTransmittedPowerdBm,
                        initialPathLossExponent, listener);
            case LMedS:
                return new LMedSRobustRangingAndRssiRadioSourceEstimator2D<>(
                        readings, initialPosition, initialTransmittedPowerdBm,
                        initialPathLossExponent, listener);
            case MSAC:
                return new MSACRobustRangingAndRssiRadioSourceEstimator2D<>(
                        readings, initialPosition, initialTransmittedPowerdBm,
                        initialPathLossExponent, listener);
            case PROSAC:
                return new PROSACRobustRangingAndRssiRadioSourceEstimator2D<>(
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator3D.java 1114
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator3D.java 1940
            final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
            final Point3D initialPosition,
            final Double initialTransmittedPowerdBm,
            final double initialPathLossExponent,
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustRangingAndRssiRadioSourceEstimator3D<>(
                        readings, initialPosition, initialTransmittedPowerdBm,
                        initialPathLossExponent, listener);
            case LMedS:
                return new LMedSRobustRangingAndRssiRadioSourceEstimator3D<>(
                        readings, initialPosition, initialTransmittedPowerdBm,
                        initialPathLossExponent, listener);
            case MSAC:
                return new MSACRobustRangingAndRssiRadioSourceEstimator3D<>(
                        readings, initialPosition, initialTransmittedPowerdBm,
                        initialPathLossExponent, listener);
            case PROSAC:
                return new PROSACRobustRangingAndRssiRadioSourceEstimator3D<>(
File Line
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator2D.java 1118
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator2D.java 1943
            final List<? extends RssiReadingLocated<S, Point2D>> readings,
            final Point2D initialPosition,
            final Double initialTransmittedPowerdBm,
            final double initialPathLossExponent,
            final RobustRssiRadioSourceEstimatorListener<S, Point2D> listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustRssiRadioSourceEstimator2D<>(
                        readings, initialPosition, initialTransmittedPowerdBm,
                        initialPathLossExponent, listener);
            case LMedS:
                return new LMedSRobustRssiRadioSourceEstimator2D<>(
                        readings, initialPosition, initialTransmittedPowerdBm,
                        initialPathLossExponent, listener);
            case MSAC:
                return new MSACRobustRssiRadioSourceEstimator2D<>(
                        readings, initialPosition, initialTransmittedPowerdBm,
                        initialPathLossExponent, listener);
            case PROSAC:
                return new PROSACRobustRssiRadioSourceEstimator2D<>(
File Line
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator3D.java 1118
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator3D.java 1945
            final List<? extends RssiReadingLocated<S, Point3D>> readings,
            final Point3D initialPosition,
            final Double initialTransmittedPowerdBm,
            final double initialPathLossExponent,
            final RobustRssiRadioSourceEstimatorListener<S, Point3D> listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustRssiRadioSourceEstimator3D<>(
                        readings, initialPosition, initialTransmittedPowerdBm,
                        initialPathLossExponent, listener);
            case LMedS:
                return new LMedSRobustRssiRadioSourceEstimator3D<>(
                        readings, initialPosition, initialTransmittedPowerdBm,
                        initialPathLossExponent, listener);
            case MSAC:
                return new MSACRobustRssiRadioSourceEstimator3D<>(
                        readings, initialPosition, initialTransmittedPowerdBm,
                        initialPathLossExponent, listener);
            case PROSAC:
                return new PROSACRobustRssiRadioSourceEstimator3D<>(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2513
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6344
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2780
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6784
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1639
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1748
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1118
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1790
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1647
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 422
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 759
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 221
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1420
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1652
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1388
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3916
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 427
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3979
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 692
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 764
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 677
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1888
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 227
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1918
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 426
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 427
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2133
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2130
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 223
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 224
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 985
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 985
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 61
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 60
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 67
public abstract class RobustKnownBiasAndFrameAccelerometerCalibrator {

    /**
     * Indicates whether by default a common z-axis is assumed for both the accelerometer
     * and gyroscope.
     */
    public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;

    /**
     * Required minimum number of measurements.
     */
    public static final int MINIMUM_MEASUREMENTS = 4;

    /**
     * Indicates that by default a linear calibrator is used for preliminary solution estimation.
     * The result obtained on each preliminary solution might be later refined.
     */
    public static final boolean DEFAULT_USE_LINEAR_CALIBRATOR = true;

    /**
     * Indicates that by default preliminary solutions are refined.
     */
    public static final boolean DEFAULT_REFINE_PRELIMINARY_SOLUTIONS = false;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD =
            RobustEstimatorMethod.LMedS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen subsamples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Contains a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     */
    protected List<StandardDeviationFrameBodyKinematics> mMeasurements;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2632
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2935
            final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4015
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4365
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, biasX, biasY, biasZ, commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, biasX, biasY, biasZ, commonAxisUsed);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2882
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3699
            final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3037
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3543
            final boolean commonAxisUsed, final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param initialMa    initial scale factors and cross coupling errors matrix.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4135
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5070
            final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4320
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4884
            final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3062
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3889
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, commonAxisUsed, initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3218
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3733
            final boolean commonAxisUsed, final Matrix initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param initialMa    initial scale factors and cross coupling errors matrix.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4337
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5302
            final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4531
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5107
            final Matrix initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias,
                        listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2541
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2504
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2499
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3731
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2987
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2543
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1665
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2505
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3689
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1130
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3749
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3794
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3859
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4218
                jacobian.setElementAt(1, 17, 0.0);

                jacobian.setElementAt(2, 0, 0.0);
                jacobian.setElementAt(2, 1, 0.0);
                jacobian.setElementAt(2, 2, 1.0);
                jacobian.setElementAt(2, 3, 0.0);
                jacobian.setElementAt(2, 4, 0.0);
                jacobian.setElementAt(2, 5, omegatruez);
                jacobian.setElementAt(2, 6, 0.0);
                jacobian.setElementAt(2, 7, 0.0);
                jacobian.setElementAt(2, 8, 0.0);
                jacobian.setElementAt(2, 9, 0.0);
                jacobian.setElementAt(2, 10, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2703
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3008
            final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4089
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4444
            final double biasZ, final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX,
                        biasY, biasZ, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(biasX,
                        biasY, biasZ, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX,
                        biasY, biasZ, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, biasX, biasY, biasZ, commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, biasX, biasY, biasZ, commonAxisUsed);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4605
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7276
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4740
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8833
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5927
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8684
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6062
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7424
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4824
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7640
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4966
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9284
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6211
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9128
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6353
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7794
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3499
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3457
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1733
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 1749
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1016
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 991
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1804
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1820
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2447
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2582
        setResult(m);
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws FittingException                         if Levenberg-Marquardt fails for numerical reasons.
     * @throws AlgebraException                         if there are numerical instabilities that prevent
     *                                                  matrix inversion.
     * @throws com.irurueta.numerical.NotReadyException never happens.
     * @throws IOException                              if world magnetic model cannot be loaded.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException, IOException {
        // The magnetometer model is:
        // bmeas = bm + (I + Mm) * btrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // bmeas = bm + (I + Mm) * btrue

        // For convergence purposes of the Levenberg-Marquardt algorithm, the
        // magnetometer model can be better expressed as:
        // bmeas = T*K*(btrue + b)
        // bmeas = M*(btrue + b)
        // bmeas = M*btrue + M*b

        //where:
        // M = I + Mm
        // bm = M*b = (I + Mm)*b --> b = M^-1*bm

        // We know that the norm of the true body magnetic flux density
        // is equal to the amount of Earth magnetic flux density at provided
        // position and timestamp
        // ||btrue|| = ||bEarth|| --> from 30 µT to 60 µT

        // Hence:
        // bmeas - M*b = M*btrue

        // M^-1 * (bmeas - M*b) = btrue

        // ||bEarth||^2 = ||btrue||^2 = (M^-1 * (bmeas - M*b))^T * (M^-1 * (bmeas - M*b))
        // ||bEarth||^2 = (bmeas - M*b)^T*(M^-1)^T * M^-1 * (bmeas - M*b)
        // ||bEarth||^2 = (bmeas - M * b)^T * ||M^-1||^2 * (bmeas - M * b)
        // ||bEarth||^2 = ||bmeas - M * b||^2 * ||M^-1||^2

        // Where:

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11 	m12 	m13]
        //     [0 		m22 	m23]
        //     [0 	 	0 		m33]


        final GradientEstimator gradientEstimator = new GradientEstimator(
                new MultiDimensionFunctionEvaluatorListener() {
                    @Override
                    public double evaluate(final double[] point)
                            throws EvaluationException {
                        return evaluateCommonAxis(point);
                    }
                });

        final Matrix initialM = Matrix.identity(
                BodyMagneticFluxDensity.COMPONENTS,
                BodyMagneticFluxDensity.COMPONENTS);
        initialM.add(getInitialMm());

        // Force initial M to be upper diagonal
        initialM.setElementAt(1, 0, 0.0);
        initialM.setElementAt(2, 0, 0.0);
        initialM.setElementAt(2, 1, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3094
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4010
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3271
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3833
            final boolean commonAxisUsed, final Matrix hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param initialMm    initial soft-iron matrix containing scale factors
     *                     and cross coupling errors.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4757
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5807
            final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4957
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5604
            final Matrix hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3203
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4119
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3380
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3942
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4866
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5916
            final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5066
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5713
            final Matrix initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron,
                        listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4599
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4659
    public static void navigateECI(final double timeInterval,
                                   final double oldX,
                                   final double oldY,
                                   final double oldZ,
                                   final CoordinateTransformation oldC,
                                   final double oldVx,
                                   final double oldVy,
                                   final double oldVz,
                                   final Acceleration fx,
                                   final Acceleration fy,
                                   final Acceleration fz,
                                   final double angularRateX,
                                   final double angularRateY,
                                   final double angularRateZ,
                                   final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                convertAccelerationToDouble(fx), convertAccelerationToDouble(fy),
                convertAccelerationToDouble(fz), angularRateX, angularRateY,
                angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECI(final Time timeInterval,
File Line
com/irurueta/navigation/frames/ECEFFrame.java 690
com/irurueta/navigation/gnss/GNSSMeasurement.java 733
        mVz = velocity.getVz();
    }

    /**
     * Gets cartesian position and velocity.
     *
     * @param result instance where cartesian position and velocity will be stored.
     */
    public void getPositionAndVelocity(final ECEFPositionAndVelocity result) {
        result.setPositionCoordinates(mX, mY, mZ);
        result.setVelocityCoordinates(mVx, mVy, mVz);
    }

    /**
     * Gets cartesian position and velocity.
     *
     * @return cartesian position and velocity.
     */
    public ECEFPositionAndVelocity getPositionAndVelocity() {
        return new ECEFPositionAndVelocity(mX, mY, mZ, mVx, mVy, mVz);
    }

    /**
     * Sets cartesian position and velocity.
     *
     * @param positionAndVelocity cartesian position and velocity.
     */
    public void setPositionAndVelocity(
            final ECEFPositionAndVelocity positionAndVelocity) {
        mX = positionAndVelocity.getX();
        mY = positionAndVelocity.getY();
        mZ = positionAndVelocity.getZ();

        mVx = positionAndVelocity.getVx();
        mVy = positionAndVelocity.getVy();
        mVz = positionAndVelocity.getVz();
    }
File Line
com/irurueta/navigation/gnss/GNSSKalmanEpochEstimator.java 305
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanEpochEstimator.java 289
            final double approxRange = Math.sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ);

            // Calculate frame rotation during signal transit time using (8.36)
            final double ceiValue = EARTH_ROTATION_RATE * approxRange / SPEED_OF_LIGHT;
            cei.setElementAt(0, 1, ceiValue);
            cei.setElementAt(1, 0, -ceiValue);

            // Predict pseudo-range using (9.165)
            satellitePosition.setElementAtIndex(0, measX);
            satellitePosition.setElementAtIndex(1, measY);
            satellitePosition.setElementAtIndex(2, measZ);

            cei.multiply(satellitePosition, deltaR);
            for (int i = 0; i < CoordinateTransformation.ROWS; i++) {
                deltaR.setElementAtIndex(i, deltaR.getElementAtIndex(i)
                        - xEstPropagated.getElementAtIndex(i));
File Line
com/irurueta/navigation/inertial/INSGNSSLooselyCoupledKalmanFilteredEstimator.java 1951
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1932
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java 1669
            gyroBiasZ = getValueOrZero(mState.getGyroBiasZ());
        } else {
            accelBiasX = 0.0;
            accelBiasY = 0.0;
            accelBiasZ = 0.0;
            gyroBiasX = 0.0;
            gyroBiasY = 0.0;
            gyroBiasZ = 0.0;
        }

        final double fx = kinematics.getFx();
        final double fy = kinematics.getFy();
        final double fz = kinematics.getFz();
        final double angularRateX = kinematics.getAngularRateX();
        final double angularRateY = kinematics.getAngularRateY();
        final double angularRateZ = kinematics.getAngularRateZ();

        mCorrectedKinematics.setSpecificForceCoordinates(
                fx - accelBiasX, fy - accelBiasY, fz - accelBiasZ);
        mCorrectedKinematics.setAngularRateCoordinates(
                angularRateX - gyroBiasX,
                angularRateY - gyroBiasY,
                angularRateZ - gyroBiasZ);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 581
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 588
            final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
        this(measurements, biasX, biasY, biasZ, commonAxisUsed);
        mListener = listener;
    }

    /**
     * Gets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<? extends FrameBodyKinematics> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body kinematics measurements taken at different
     *                     frames (positions, orientations and velocities).
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setMeasurements(final Collection<? extends FrameBodyKinematics> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    @Override
    public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3160
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3676
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4140
                return initial;

            }

            @Override
            public void evaluate(
                    final int i, final double[] point, final double[] result,
                    final double[] params, final Matrix jacobian) {
                // We know that:
                //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                //  fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
                //  fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters sx, sy, sz,
                // mxy, mxz, myx, myz, mzx and mzy is:

                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myx) = 0.0
                // d(fmeasx)/d(myz) = 0.0
                // d(fmeasx)/d(mzx) = 0.0
                // d(fmeasx)/d(mzy) = 0.0

                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myx) = ftruex
                // d(fmeasy)/d(myz) = ftruez
                // d(fmeasy)/d(mzx) = 0.0
                // d(fmeasy)/d(mzy) = 0.0

                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myx) = 0.0
                // d(fmeasz)/d(myz) = 0.0
                // d(fmeasz)/d(mzx) = ftruex
                // d(fmeasz)/d(mzy) = ftruey

                final double sx = params[0];
                final double sy = params[1];
                final double sz = params[2];

                final double mxy = params[3];
                final double mxz = params[4];
                final double myx = params[5];
                final double myz = params[6];
                final double mzx = params[7];
                final double mzy = params[8];

                final double ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6344
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6784
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1639
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1748
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1118
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1790
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2541
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2499
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3731
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2543
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1665
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2505
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3689
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1130
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3749
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3794
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1733
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 1749
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1016
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 991
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1804
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1820
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setInitialMa(final Matrix initialMa) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3431
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3713
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4099
                return initial;
            }

            @Override
            public void evaluate(
                    final int i, final double[] point, final double[] result,
                    final double[] params, final Matrix jacobian) {
                // We know that:
                //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                //  fmeasy = by + ftruey + sy * ftruey + myz * ftruez
                //  fmeasz = bz + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
                // sz, mxy, mxz, myz

                // d(fmeasx)/d(bx) = 1.0
                // d(fmeasx)/d(by) = 0.0
                // d(fmeasx)/d(bz) = 0.0
                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myz) = 0.0

                // d(fmeasy)/d(bx) = 0.0
                // d(fmeasy)/d(by) = 1.0
                // d(fmeasy)/d(bz) = 0.0
                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myz) = ftruez

                // d(fmeasz)/d(bx) = 0.0
                // d(fmeasz)/d(by) = 0.0
                // d(fmeasz)/d(bz) = 1.0
                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myz) = 0.0

                final double bx = params[0];
                final double by = params[1];
                final double bz = params[2];

                final double sx = params[3];
                final double sy = params[4];
                final double sz = params[5];

                final double mxy = params[6];
                final double mxz = params[7];
                final double myz = params[8];

                final double ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3629
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4022
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public int getNumberOfVariables() {
                // The multivariate function returns the components of measured specific force
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final double[] initial = new double[GENERAL_UNKNOWNS];

                initial[0] = mInitialBiasX;
                initial[1] = mInitialBiasY;
                initial[2] = mInitialBiasZ;

                initial[3] = mInitialSx;
                initial[4] = mInitialSy;
                initial[5] = mInitialSz;

                initial[6] = mInitialMxy;
                initial[7] = mInitialMxz;
                initial[8] = mInitialMyx;
                initial[9] = mInitialMyz;
                initial[10] = mInitialMzx;
                initial[11] = mInitialMzy;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7055
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1582
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 887
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 268
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 898
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 814
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1937
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 275
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 1968
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 256
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1033
        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1647
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 422
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 840
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 851
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1420
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1652
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1388
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3916
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 427
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3979
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 426
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 427
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2133
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2130
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1752
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1771
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 759
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 221
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 692
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 764
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 677
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1888
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 227
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1918
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 223
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 224
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 985
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 985
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return mComputeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4088
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4837
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias. This must have length 3 and is
     *                      expressed in meters per squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1527
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2198
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2842
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4302
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4289
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5059
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial accelerometer bias to be used to find a solution.
     *                      This must have length 3 and is expressed in meters per
     *                      squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4026
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4122
    private double evaluateGeneralWithGDependentCrossBiases(
            final int i, final double[] params)
            throws EvaluationException {

        final double bx = params[0];
        final double by = params[1];
        final double bz = params[2];

        final double m11 = params[3];
        final double m21 = params[4];
        final double m31 = params[5];

        final double m12 = params[6];
        final double m22 = params[7];
        final double m32 = params[8];

        final double m13 = params[9];
        final double m23 = params[10];
        final double m33 = params[11];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 902
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 1047
            a.setElementAt(i, 17, fTrueZ);

            b.setElementAtIndex(i, omegaMeasZ - omegaTrueZ);
            i++;
        }

        final Matrix unknowns = Utils.solve(a, b);

        final double bx = unknowns.getElementAtIndex(0);
        final double by = unknowns.getElementAtIndex(1);
        final double bz = unknowns.getElementAtIndex(2);
        final double sx = unknowns.getElementAtIndex(3);
        final double sy = unknowns.getElementAtIndex(4);
        final double sz = unknowns.getElementAtIndex(5);
        final double mxy = unknowns.getElementAtIndex(6);
        final double mxz = unknowns.getElementAtIndex(7);
        final double myz = unknowns.getElementAtIndex(8);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 819
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 804
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 771
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 756
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1557
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1525
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1579
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1549
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 850
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 835
            innerEstimator.setStopThreshold(mStopThreshold);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5561
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6883
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8505
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9919
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5757
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7074
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8305
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9713
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5830
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7212
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8934
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10430
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6029
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7421
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8719
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10209
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases, initialBias,
                        initialMg, initialGg, accelerometerBias,
                        accelerometerMa);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 337
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1114
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 288
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1064
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 565
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2268
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 588
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2292
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 379
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1142
            innerEstimator.setStopThreshold(mStopThreshold);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 338
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 354
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1112
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1115
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 353
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1111
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 303
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1063
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 561
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2269
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 589
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2294
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 379
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1140
            innerEstimator.setStopThreshold(mStopThreshold);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 289
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 304
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1064
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1065
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 380
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 380
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1141
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1143
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4707
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5554
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4816
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5663
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8383
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8419
    public static void navigateECEF(final double timeInterval,
                                    final double oldX,
                                    final double oldY,
                                    final double oldZ,
                                    final CoordinateTransformation oldC,
                                    final ECEFVelocity oldVelocity,
                                    final BodyKinematics kinematics,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                kinematics.getFx(), kinematics.getFy(), kinematics.getFz(),
                kinematics.getAngularRateX(), kinematics.getAngularRateY(),
                kinematics.getAngularRateZ(), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/lateration/HomogeneousLinearLeastSquaresLateration2DSolver.java 168
com/irurueta/navigation/lateration/RobustLateration2DSolver.java 1417
    }

    /**
     * Internally sets circles defining positions and euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 3.
     */
    private void internalSetCircles(final Circle[] circles) {
        if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final Point2D[] positions = new Point2D[circles.length];
        final double[] distances = new double[circles.length];
        for (int i = 0; i < circles.length; i++) {
            final Circle circle = circles[i];
            positions[i] = circle.getCenter();
            distances[i] = circle.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }
File Line
com/irurueta/navigation/lateration/HomogeneousLinearLeastSquaresLateration3DSolver.java 168
com/irurueta/navigation/lateration/InhomogeneousLinearLeastSquaresLateration3DSolver.java 168
com/irurueta/navigation/lateration/RobustLateration3DSolver.java 1419
    }

    /**
     * Internally sets spheres defining positions and euclidean distances.
     *
     * @param spheres spheres defining positions and distances.
     * @throws IllegalArgumentException if spheres is null or length of array of spheres
     *                                  is less than 4.
     */
    private void internalSetSpheres(final Sphere[] spheres) {
        if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final Point3D[] positions = new Point3D[spheres.length];
        final double[] distances = new double[spheres.length];
        for (int i = 0; i < spheres.length; i++) {
            final Sphere sphere = spheres[i];
            positions[i] = sphere.getCenter();
            distances[i] = sphere.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }
File Line
com/irurueta/navigation/lateration/LaterationSolver.java 246
com/irurueta/navigation/lateration/RobustLaterationSolver.java 791
    public abstract int getMinRequiredPositionsAndDistances();

    /**
     * Internally sets known positions and euclidean distances.
     * If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
     *
     * @param positions known positios of static nodes.
     * @param distances euclidean distances from static nodes to mobile node.
     * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
     *                                  length is smaller than required (2 points).
     */
    protected void internalSetPositionsAndDistances(
            final P[] positions, final double[] distances) {
        if (positions == null || distances == null) {
            throw new IllegalArgumentException();
        }

        if (positions.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        if (positions.length != distances.length) {
            throw new IllegalArgumentException();
        }

        mPositions = positions;
        mDistances = distances;

        //fix distances if needed
        for (int i = 0; i < mDistances.length; i++) {
            if (mDistances[i] < EPSILON) {
                mDistances[i] = EPSILON;
            }
        }
    }
File Line
com/irurueta/navigation/gnss/GNSSMeasurement.java 733
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 1634
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 1808
        mVz = ecefVelocity.getVz();
    }

    /**
     * Gets ECEF position and velocity of satellite.
     *
     * @param result instance where position and velocity will be stored.
     */
    public void getPositionAndVelocity(final ECEFPositionAndVelocity result) {
        result.setPositionCoordinates(mX, mY, mZ);
        result.setVelocityCoordinates(mVx, mVy, mVz);
    }

    /**
     * Gets ECEF position and velocity of satellite.
     *
     * @return ECEF position and velocity of satellite.
     */
    public ECEFPositionAndVelocity getPositionAndVelocity() {
        return new ECEFPositionAndVelocity(mX, mY, mZ, mVx, mVy, mVz);
    }

    /**
     * Sets ECEF position and velocity of satellite.
     *
     * @param positionAndVelocity ECEF position and velocity of satellite.
     */
    public void setPositionAndVelocity(final ECEFPositionAndVelocity positionAndVelocity) {
        mX = positionAndVelocity.getX();
        mY = positionAndVelocity.getY();
        mZ = positionAndVelocity.getZ();

        mVx = positionAndVelocity.getVx();
        mVy = positionAndVelocity.getVy();
        mVz = positionAndVelocity.getVz();
    }

    /**
     * Copies this instance data into provided instance.
     *
     * @param output destination instance where data will be copied to.
     */
    public void copyTo(GNSSMeasurement output) {
File Line
com/irurueta/navigation/indoor/position/PROMedSRobustRssiPositionEstimator3D.java 370
com/irurueta/navigation/indoor/position/PROSACRobustMixedPositionEstimator3D.java 366
com/irurueta/navigation/indoor/position/PROSACRobustRangingAndRssiPositionEstimator3D.java 370
com/irurueta/navigation/indoor/position/PROSACRobustRangingPositionEstimator3D.java 366
            final RobustRssiPositionEstimatorListener<Point3D> listener) {
        this(sources, fingerprint, listener);
        internalSetSourceQualityScores(sourceQualityScores);
        internalSetFingerprintReadingsQualityScores(fingerprintReadingQualityScores);
    }

    /**
     * Returns quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each radio source.
     */
    public double[] getSourceQualityScores() {
        return mSourceQualityScores;
    }

    /**
     * Sets quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the radio source.
     *
     * @param sourceQualityScores quality scores corresponding to each radio source.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setSourceQualityScores(final double[] sourceQualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSourceQualityScores(sourceQualityScores);
    }

    /**
     * Gets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each reading within provided
     * fingerprint.
     */
    public double[] getFingerprintReadingsQualityScores() {
        return mFingerprintReadingsQualityScores;
    }

    /**
     * Sets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @param fingerprintReadingsQualityScores quality scores corresponding to each
     *                                         reading within provided fingerprint.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingRadioSourceEstimator2D.java 175
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingRadioSourceEstimator3D.java 176
            final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Robustly estimates position for a radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException,
            RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingRadioSourceEstimator2D.java 153
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingRadioSourceEstimator3D.java 151
            final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if robust estimator is locked because an
     *                                  estimation is already in progress.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Robustly estimates position for a radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException,
            RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7063
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1535
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2206
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2996
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4402
    public double[] getEstimatedBiases() {
        return mEstimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    public boolean getEstimatedBiases(final double[] result) {
        if (mEstimatedBiases != null) {
            System.arraycopy(mEstimatedBiases, 0, result,
                    0, mEstimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    public Matrix getEstimatedBiasesAsMatrix() {
        return mEstimatedBiases != null ? Matrix.newFromArray(mEstimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    public boolean getEstimatedBiasesAsMatrix(final Matrix result)
            throws WrongSizeException {
        if (mEstimatedBiases != null) {
            result.fromArray(mEstimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4605
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8684
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5927
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7276
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4824
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9128
            final ECEFPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6211
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7640
            final NEDPosition position,
            final double turntableRotationRate,
            final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/frames/ECEFFrame.java 690
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 1634
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 1808
        mVz = velocity.getVz();
    }

    /**
     * Gets cartesian position and velocity.
     *
     * @param result instance where cartesian position and velocity will be stored.
     */
    public void getPositionAndVelocity(final ECEFPositionAndVelocity result) {
        result.setPositionCoordinates(mX, mY, mZ);
        result.setVelocityCoordinates(mVx, mVy, mVz);
    }

    /**
     * Gets cartesian position and velocity.
     *
     * @return cartesian position and velocity.
     */
    public ECEFPositionAndVelocity getPositionAndVelocity() {
        return new ECEFPositionAndVelocity(mX, mY, mZ, mVx, mVy, mVz);
    }

    /**
     * Sets cartesian position and velocity.
     *
     * @param positionAndVelocity cartesian position and velocity.
     */
    public void setPositionAndVelocity(
            final ECEFPositionAndVelocity positionAndVelocity) {
        mX = positionAndVelocity.getX();
        mY = positionAndVelocity.getY();
        mZ = positionAndVelocity.getZ();

        mVx = positionAndVelocity.getVx();
        mVy = positionAndVelocity.getVy();
        mVz = positionAndVelocity.getVz();
    }
File Line
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1086
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1178
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1612
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1685
    public void fix(
            final double measuredAngularRateX,
            final double measuredAngularRateY,
            final double measuredAngularRateZ,
            final double trueFx,
            final double trueFy,
            final double trueFz,
            final double biasX, final double biasY, final double biasZ,
            final double sx, final double sy, final double sz,
            final double mxy, final double mxz,
            final double myx, final double myz,
            final double mzx, final double mzy,
            final double g11, final double g21, final double g31,
            final double g12, final double g22, final double g32,
            final double g13, final double g23, final double g33,
File Line
com/irurueta/navigation/inertial/calibration/IMUBiasEstimator.java 3991
com/irurueta/navigation/inertial/calibration/IMUNoiseEstimator.java 1254
    }

    /**
     * Adds a sample of body kinematics (accelerometer + gyroscope readings) obtained
     * from an IMU.
     * If estimator is already finished, provided sample will be ignored.
     *
     * @param kinematics kinematics instance to be added and processed.
     * @return true if provided kinematics instance has been processed, false if it has
     * been ignored.
     * @throws LockedException if estimator is currently running.
     */
    public boolean addBodyKinematics(final BodyKinematics kinematics)
            throws LockedException {

        if (mRunning) {
            throw new LockedException();
        }

        if (isFinished()) {
            return true;
        }

        mRunning = true;

        if (mLastBodyKinematics == null && mListener != null) {
            mListener.onStart(this);
        }

        final double fx = kinematics.getFx();
        final double fy = kinematics.getFy();
        final double fz = kinematics.getFz();
        final double angularRateX = kinematics.getAngularRateX();
        final double angularRateY = kinematics.getAngularRateY();
        final double angularRateZ = kinematics.getAngularRateZ();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1459
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1502
            b.setElementAtIndex(i, fMeasZ - fTrueZ - mBiasZ);
            i++;
        }

        final Matrix unknowns = Utils.solve(a, b);

        final double sx = unknowns.getElementAtIndex(0);
        final double sy = unknowns.getElementAtIndex(1);
        final double sz = unknowns.getElementAtIndex(2);
        final double mxy = unknowns.getElementAtIndex(3);
        final double mxz = unknowns.getElementAtIndex(4);
        final double myx = unknowns.getElementAtIndex(5);
        final double myz = unknowns.getElementAtIndex(6);
        final double mzx = unknowns.getElementAtIndex(7);
        final double mzy = unknowns.getElementAtIndex(8);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2513
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2780
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2541
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2499
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3731
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2543
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1665
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2505
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3689
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1130
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3749
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3794
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1733
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 1749
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1016
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 991
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1804
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1820
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 5732
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1018
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1115
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1866
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3098
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1047
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1872
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3058
                bias, initialMa, listener);
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return x-coordinate of known accelerometer bias.
     */
    public double getBiasX() {
        return mBiasX;
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasX(final double biasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasX = biasX;
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return y-coordinate of known accelerometer bias.
     */
    public double getBiasY() {
        return mBiasY;
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasY(final double biasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasY = biasY;
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return z-coordinate of known accelerometer bias.
     */
    public double getBiasZ() {
        return mBiasZ;
    }

    /**
     * Sets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBiasZ(final double biasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mBiasZ = biasZ;
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @return x-coordinate of known accelerometer bias.
     */
    public Acceleration getBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6344
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6784
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1639
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1748
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1118
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1790
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2504
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2987
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3499
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3457
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS ||
                result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mInitialSx);
        result.setElementAtIndex(1, mInitialMyx);
        result.setElementAtIndex(2, mInitialMzx);

        result.setElementAtIndex(3, mInitialMxy);
        result.setElementAtIndex(4, mInitialSy);
        result.setElementAtIndex(5, mInitialMzy);

        result.setElementAtIndex(6, mInitialMxz);
        result.setElementAtIndex(7, mInitialMyz);
        result.setElementAtIndex(8, mInitialSz);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6962
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7592
        setResult(m);
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws FittingException                         if Levenberg-Marquardt fails for numerical reasons.
     * @throws AlgebraException                         if there are numerical instabilities that prevent
     *                                                  matrix inversion.
     * @throws com.irurueta.numerical.NotReadyException never happens.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // For convergence purposes of the Levenberg-Marquardt algorithm, the
        // accelerometer model can be better expressed as:
        // fmeas = T*K*(ftrue + b)
        // fmeas = M*(ftrue + b)
        // fmeas = M*ftrue + M*b

        //where:
        // M = I + Ma
        // ba = M*b = (I + Ma)*b --> b = M^-1*ba

        // We know that the norm of the true specific force is equal to the amount
        // of gravity at a certain Earth position
        // ||ftrue|| = ||g|| ~ 9.81 m/s^2

        // Hence:
        // fmeas - M*b = M*ftrue

        // M^-1 * (fmeas - M*b) = ftrue

        // ||g||^2 = ||ftrue||^2 = (M^-1 * (fmeas - M*b))^T * (M^-1 * (fmeas - M*b))
        // ||g||^2 = (fmeas - M*b)^T*(M^-1)^T * M^-1 * (fmeas - M*b)
        // ||g||^2 = (fmeas - M * b)^T * ||M^-1||^2 * (fmeas - M * b)
        // ||g||^2 = ||fmeas - M * b||^2 * ||M^-1||^2

        // Where:

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11 	m12 	m13]
        //     [0 		m22 	m23]
        //     [0 	 	0 		m33]

        // Notice that bias b is known, hence only terms in matrix M need to be estimated

        final GradientEstimator gradientEstimator = new GradientEstimator(
                new MultiDimensionFunctionEvaluatorListener() {
                    @Override
                    public double evaluate(double[] point) throws EvaluationException {
                        return evaluateCommonAxis(point);
                    }
                });

        final Matrix initialM = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        initialM.add(getInitialMa());

        // Force initial M to be upper diagonal
        initialM.setElementAt(1, 0, 0.0);
        initialM.setElementAt(2, 0, 0.0);
        initialM.setElementAt(2, 1, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 854
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 966
            a.setElementAt(i, 5, fTrueZ);

            b.setElementAtIndex(i, fMeasZ - fTrueZ);
            i++;
        }

        final Matrix unknowns = Utils.solve(a, b);

        final double bx = unknowns.getElementAtIndex(0);
        final double by = unknowns.getElementAtIndex(1);
        final double bz = unknowns.getElementAtIndex(2);
        final double sx = unknowns.getElementAtIndex(3);
        final double sy = unknowns.getElementAtIndex(4);
        final double sz = unknowns.getElementAtIndex(5);
        final double mxy = unknowns.getElementAtIndex(6);
        final double mxz = unknowns.getElementAtIndex(7);
        final double myz = unknowns.getElementAtIndex(8);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6172
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1132
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 1888
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 1890
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3096
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3141
                initialBias, initialMa, listener);
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    public Acceleration getInitialBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7804
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4123
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5742
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2801
    private double evaluateGeneral(final double[] params) throws EvaluationException {
        final double bx = params[0];
        final double by = params[1];
        final double bz = params[2];

        final double m11 = params[3];
        final double m21 = params[4];
        final double m31 = params[5];

        final double m12 = params[6];
        final double m22 = params[7];
        final double m32 = params[8];

        final double m13 = params[9];
        final double m23 = params[10];
        final double m33 = params[11];

        return evaluate(bx, by, bz, m11, m21, m31, m12, m22, m32,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 877
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 831
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1777
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1804
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 908
            innerEstimator.setStopThreshold(mStopThreshold);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 878
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 962
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 343
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 973
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 891
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 2013
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 346
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 2044
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 961
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 342
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 972
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 915
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 294
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 925
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1886
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 553
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1905
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1908
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 581
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1932
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 996
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 375
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 1007
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 890
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 2012
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 345
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 2043
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 842
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1962
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 302
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1994
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1788
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 4050
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 561
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 4113
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1811
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 4074
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 588
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 4139
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 919
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 2043
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 382
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 2073
            innerEstimator.setStopThreshold(mStopThreshold);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 832
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 916
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 295
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 926
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 843
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1963
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 303
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1995
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 909
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 997
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 376
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 1008
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 920
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 2044
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 383
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 2074
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1272
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2415
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1300
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2421
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @return known accelerometer bias as a column matrix.
     */
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS
                || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, mBiasX);
        result.setElementAtIndex(1, mBiasY);
        result.setElementAtIndex(2, mBiasZ);
    }

    /**
     * Sets known accelerometer bias as a column matrix.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setBias(final Matrix bias) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2552
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2861
            final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param measurements list of body kinematics measurements with standard
     *                     deviations taken at different frames (positions, orientations
     *                     and velocities).
     * @param biasX        known x coordinate of accelerometer bias expressed in meters per
     *                     squared second (m/s^2).
     * @param biasY        known y coordinate of accelerometer bias expressed in meters per
     *                     squared second (m/s^2).
     * @param biasZ        known z coordinate of accelerometer bias expressed in meters per
     *                     squared second (m/s^2).
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4778
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 5102
            final double[] bias, final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, bias, commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, bias, commonAxisUsed);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param bias           known accelerometer bias.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided quality scores length is smaller than
     *                                  4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores,
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double[] bias, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4822
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 5146
            final double[] bias, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, bias, commonAxisUsed,
                        listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, bias, commonAxisUsed,
                        listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param bias          known accelerometer bias.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided quality scores length is smaller than
     *                                  4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3951
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4698
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   list of body kinematics measurements taken at a given position with
     *                       different unknown orientations and containing the standard deviations
     *                       of accelerometer and gyroscope measurements.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope. If true 7 samples are
     *                       required, otherwise 10.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum number of
     *                                  required samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3996
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4743
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   list of body kinematics measurements taken at a given position with
     *                       different unknown orientations and containing the standard deviations
     *                       of accelerometer and gyroscope measurements.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope. If true 7 samples are
     *                       required, otherwise 10.
     * @param listener       listener to be notified of events such as when estimation
     *                       starts, ends or its progress significantly changes.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum number of
     *                                  required samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4090
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4277
            final double[] bias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias. This must have length 3 and is
     *                      expressed in meters per squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4275
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5024
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4839
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5026
            final double[] bias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias. This must have length 3 and is
     *                      expressed in meters per squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 6222
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 6485
            mTmp3.setElementAtIndex(2, fmeasZ - mBiasZ);

            mTmp2.multiply(mTmp3, mTmp4);

            final double norm = Utils.normF(mTmp4);
            final double diff = mGravityNorm - norm;

            return diff * diff;

        } catch (final AlgebraException e) {
            return Double.MAX_VALUE;
        }
    }

    /**
     * Computes a preliminary solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminary solution will be stored.
     */
    protected void computePreliminarySolutions(final int[] samplesIndices,
                                               final List<PreliminaryResult> solutions) {

        final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();

        for (final int samplesIndex : samplesIndices) {
            measurements.add(mMeasurements.get(samplesIndex));
        }

        try {
            final PreliminaryResult result = new PreliminaryResult();
            result.mEstimatedMa = getInitialMa();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4151
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4921
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   list of body kinematics measurements taken at a given position with
     *                       different unknown orientations and containing the standard deviations
     *                       of accelerometer and gyroscope measurements.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope. If true 10 samples are
     *                       required, otherwise 13.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum number of
     *                                  required samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4196
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4966
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   list of body kinematics measurements taken at a given position with
     *                       different unknown orientations and containing the standard deviations
     *                       of accelerometer and gyroscope measurements.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope. If true 10 samples are
     *                       required, otherwise 13.
     * @param listener       listener to be notified of events such as when estimation
     *                       starts, ends or its progress significantly changes.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum number of
     *                                  required samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4291
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4488
            final double[] initialBias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial accelerometer bias to be used to find a solution.
     *                      This must have length 3 and is expressed in meters per
     *                      squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4486
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5256
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5061
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5258
            final double[] initialBias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial accelerometer bias to be used to find a solution.
     *                      This must have length 3 and is expressed in meters per
     *                      squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3601
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4624
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2623
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2933
            final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param measurements list of body kinematics measurements with standard
     *                     deviations taken at different frames (positions, orientations
     *                     and velocities).
     * @param biasX        known x coordinate of gyroscope bias expressed in radians per
     *                     second (rad/s).
     * @param biasY        known y coordinate of gyroscope bias expressed in radians per
     *                     second (rad/s).
     * @param biasZ        known z coordinate of gyroscope bias expressed in radians per
     *                     second (rad/s).
     * @param method       robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4858
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5180
            final double[] bias, final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, bias, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, bias, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, bias, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(qualityScores,
                        measurements, bias, commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(qualityScores,
                        measurements, bias, commonAxisUsed);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param bias           known gyroscope bias.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided quality scores length is smaller
     *                                  than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double[] bias, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4902
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5224
            final double[] bias, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, measurements, bias, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, measurements, bias, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param bias          known gyroscope bias.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided quality scores length is smaller
     *                                  than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3395
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4418
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4968
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6290
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7842
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9250
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5128
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6450
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7671
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9076
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5208
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6590
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8231
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9718
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5371
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6753
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8054
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9541
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 862
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1007
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1954
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2100
    private void calibrateCommonAxis() throws AlgebraException, IOException {
        // The magnetometer model is:
        // mBmeas = bm + (I + Mm) * mBtrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // mBmeas = bm + (I + Mm) * mBtrue

        // Hence:
        //  [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        //  [mBmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [mBtruey]
        //  [mBmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [mBtruez]

        // where myx = mzx = mzy = 0

        // Hence:
        //  [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        //  [mBmeasy] = [by]     [0  1   0]   [0     sy  myz]    [mBtruey]
        //  [mBmeasz] = [bz]     [0  0   1]   [0     0   sz ]    [mBtruez]

        //  [mBmeasx] = [bx] +   [1+sx   mxy     mxz ][mBtruex]
        //  [mBmeasy]   [by]     [0      1+sy    myz ][mBtruey]
        //  [mBmeasz]   [bz]     [0      0       1+sz][mBtruez]

        //  mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy = by + (1+sy) * mBtruey + myz * mBtruez
        //  mBmeasz = bz + (1+sz) * mBtruez

        // Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myz
        // Reordering:
        //  mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy = by + mBtruey + sy * mBtruey + myz * mBtruez
        //  mBmeasz = bz + mBtruez + sz * mBtruez

        //  mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        //  mBmeasy - mBtruey = by + sy * mBtruey + myz * mBtruez
        //  mBmeasz - mBtruez = bz + sz * mBtruez

        // [1   0   0   mBtruex  0        0        mBtruey  mBtruez  0      ][bx ] = [mBmeasx - mBtruex]
        // [0   1   0   0        mBtruey  0        0        0        mBtruez][by ]   [mBmeasy - mBtruey]
        // [0   0   1   0        0        mBtruez  0        0        0      ][bz ]   [mBmeasz - mBtruez]
        //                                                                   [sx ]
        //                                                                   [sy ]
        //                                                                   [sz ]
        //                                                                   [mxy]
        //                                                                   [mxz]
        //                                                                   [myz]

        final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
        if (mMagneticModel != null) {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(mMagneticModel);
        } else {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
        }

        final BodyMagneticFluxDensity expectedMagneticFluxDensity =
                new BodyMagneticFluxDensity();
        final NEDFrame nedFrame = new NEDFrame();
        final NEDMagneticFluxDensity earthB = new NEDMagneticFluxDensity();
        final CoordinateTransformation cbn = new CoordinateTransformation(
                FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final CoordinateTransformation cnb = new CoordinateTransformation(
                FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);

        final int rows = EQUATIONS_PER_MEASUREMENT * mMeasurements.size();
        final Matrix a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 979
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1123
            a.setElementAt(i, 5, bTrueZ);

            b.setElementAtIndex(i, bMeasZ - bTrueZ);
            i++;
        }

        final Matrix unknowns = Utils.solve(a, b);

        final double bx = unknowns.getElementAtIndex(0);
        final double by = unknowns.getElementAtIndex(1);
        final double bz = unknowns.getElementAtIndex(2);
        final double sx = unknowns.getElementAtIndex(3);
        final double sy = unknowns.getElementAtIndex(4);
        final double sz = unknowns.getElementAtIndex(5);
        final double mxy = unknowns.getElementAtIndex(6);
        final double mxz = unknowns.getElementAtIndex(7);
        final double myz = unknowns.getElementAtIndex(8);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1207
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 474
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1278
    }

    /**
     * Gets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known x-coordinate of magnetometer hard-iron bias.
     */
    public double getHardIronX() {
        return mHardIronX;
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronX known x-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setHardIronX(final double hardIronX)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mHardIronX = hardIronX;
    }

    /**
     * Gets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known y-coordinate of magnetometer hard-iron bias.
     */
    public double getHardIronY() {
        return mHardIronY;
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronY known y-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setHardIronY(final double hardIronY)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mHardIronY = hardIronY;
    }

    /**
     * Gets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known z-coordinate of magnetometer hard-iron bias.
     */
    public double getHardIronZ() {
        return mHardIronZ;
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in meters Teslas (T).
     *
     * @param hardIronZ known z-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setHardIronZ(final double hardIronZ)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mHardIronZ = hardIronZ;
    }

    /**
     * Sets known hard-iron bias coordinates of magnetometer expressed in
     * Teslas (T).
     *
     * @param hardIronX x-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronY y-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronZ z-coordinate of magnetometer
     *                  known hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setHardIron(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4564
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5411
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param method         robust estimator method
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4610
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5457
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4709
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4913
            final double[] hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4911
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5758
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5556
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5760
            final double[] hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4673
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5520
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param method         robust estimator method
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4719
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5566
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4818
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5022
            final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5020
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5867
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5665
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5869
            final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11823
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11885
    public static ECEFFrame navigateECEFAndReturnNew(final double timeInterval,
                                                     final double oldX,
                                                     final double oldY,
                                                     final double oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final double oldVx,
                                                     final double oldVy,
                                                     final double oldVz,
                                                     final double fx,
                                                     final double fy,
                                                     final double fz,
                                                     final double angularRateX,
                                                     final double angularRateY,
                                                     final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12869
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12931
    public static ECEFFrame navigateECEFAndReturnNew(final double timeInterval,
                                                     final Distance oldX,
                                                     final Distance oldY,
                                                     final Distance oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final double oldVx,
                                                     final double oldVy,
                                                     final double oldVz,
                                                     final double fx,
                                                     final double fy,
                                                     final double fz,
                                                     final double angularRateX,
                                                     final double angularRateY,
                                                     final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 13253
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 13315
    public static ECEFFrame navigateECEFAndReturnNew(final double timeInterval,
                                                     final double oldX,
                                                     final double oldY,
                                                     final double oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final Speed oldSpeedX,
                                                     final Speed oldSpeedY,
                                                     final Speed oldSpeedZ,
                                                     final double fx,
                                                     final double fy,
                                                     final double fz,
                                                     final double angularRateX,
                                                     final double angularRateY,
                                                     final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY,
                oldSpeedZ, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 13636
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 13695
    public static ECEFFrame navigateECEFAndReturnNew(final double timeInterval,
                                                     final double oldX,
                                                     final double oldY,
                                                     final double oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final double oldVx,
                                                     final double oldVy,
                                                     final double oldVz,
                                                     final Acceleration fx,
                                                     final Acceleration fy,
                                                     final Acceleration fz,
                                                     final double angularRateX,
                                                     final double angularRateY,
                                                     final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy,
                fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 14055
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 14114
    public static ECEFFrame navigateECEFAndReturnNew(final double timeInterval,
                                                     final double oldX,
                                                     final double oldY,
                                                     final double oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final double oldVx,
                                                     final double oldVy,
                                                     final double oldVz,
                                                     final double fx,
                                                     final double fy,
                                                     final double fz,
                                                     final AngularSpeed angularRateX,
                                                     final AngularSpeed angularRateY,
                                                     final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy,
                fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 14474
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 14537
    public static ECEFFrame navigateECEFAndReturnNew(final double timeInterval,
                                                     final Distance oldX,
                                                     final Distance oldY,
                                                     final Distance oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final Speed oldSpeedX,
                                                     final Speed oldSpeedY,
                                                     final Speed oldSpeedZ,
                                                     final double fx,
                                                     final double fy,
                                                     final double fz,
                                                     final double angularRateX,
                                                     final double angularRateY,
                                                     final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY,
                oldSpeedZ, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 14594
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 14651
    public static ECEFFrame navigateECEFAndReturnNew(final double timeInterval,
                                                     final Distance oldX,
                                                     final Distance oldY,
                                                     final Distance oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final Speed oldSpeedX,
                                                     final Speed oldSpeedY,
                                                     final Speed oldSpeedZ,
                                                     final Acceleration fx,
                                                     final Acceleration fy,
                                                     final Acceleration fz,
                                                     final AngularSpeed angularRateX,
                                                     final AngularSpeed angularRateY,
                                                     final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY,
                oldSpeedZ, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 14988
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 15044
    public static ECEFFrame navigateECEFAndReturnNew(final double timeInterval,
                                                     final double oldX,
                                                     final double oldY,
                                                     final double oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final double oldVx,
                                                     final double oldVy,
                                                     final double oldVz,
                                                     final Acceleration fx,
                                                     final Acceleration fy,
                                                     final Acceleration fz,
                                                     final AngularSpeed angularRateX,
                                                     final AngularSpeed angularRateY,
                                                     final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx,
                fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5667
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5730
    public static ECIFrame navigateECIAndReturnNew(final double timeInterval,
                                                   final double oldX,
                                                   final double oldY,
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVx,
                                                   final double oldVy,
                                                   final double oldVz,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6062
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6124
    public static ECIFrame navigateECIAndReturnNew(final double timeInterval,
                                                   final Distance oldX,
                                                   final Distance oldY,
                                                   final Distance oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVx,
                                                   final double oldVy,
                                                   final double oldVz,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6270
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6332
    public static ECIFrame navigateECIAndReturnNew(final double timeInterval,
                                                   final double oldX,
                                                   final double oldY,
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedX,
                                                   final Speed oldSpeedY,
                                                   final Speed oldSpeedZ,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6475
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6534
    public static ECIFrame navigateECIAndReturnNew(final double timeInterval,
                                                   final double oldX,
                                                   final double oldY,
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVx,
                                                   final double oldVy,
                                                   final double oldVz,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6593
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6652
    public static ECIFrame navigateECIAndReturnNew(final double timeInterval,
                                                   final double oldX,
                                                   final double oldY,
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVx,
                                                   final double oldVy,
                                                   final double oldVz,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6714
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6776
    public static ECIFrame navigateECIAndReturnNew(final double timeInterval,
                                                   final Distance oldX,
                                                   final Distance oldY,
                                                   final Distance oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedX,
                                                   final Speed oldSpeedY,
                                                   final Speed oldSpeedZ,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6832
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6888
    public static ECIFrame navigateECIAndReturnNew(final double timeInterval,
                                                   final Distance oldX,
                                                   final Distance oldY,
                                                   final Distance oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedX,
                                                   final Speed oldSpeedY,
                                                   final Speed oldSpeedZ,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6944
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 7000
    public static ECIFrame navigateECIAndReturnNew(final double timeInterval,
                                                   final double oldX,
                                                   final double oldY,
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVx,
                                                   final double oldVy,
                                                   final double oldVz,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11739
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11802
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final double oldLatitude,
                                                   final double oldLongitude,
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY,
                angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 12493
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 12556
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final Angle oldLatitude,
                                                   final Angle oldLongitude,
                                                   final Distance oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 12885
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 12948
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final double oldLatitude,
                                                   final double oldLongitude,
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedN,
                                                   final Speed oldSpeedE,
                                                   final Speed oldSpeedD,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX,
                angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13284
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13344
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final double oldLatitude,
                                                   final double oldLongitude,
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13724
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13784
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final double oldLatitude,
                                                   final double oldLongitude,
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14167
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14230
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final Angle oldLatitude,
                                                   final Angle oldLongitude,
                                                   final Distance oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedN,
                                                   final Speed oldSpeedE,
                                                   final Speed oldSpeedD,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight,
                oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14287
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14344
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final Angle oldLatitude,
                                                   final Angle oldLongitude,
                                                   final Distance oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedN,
                                                   final Speed oldSpeedE,
                                                   final Speed oldSpeedD,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight,
                oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14401
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14458
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final double oldLatitude,
                                                   final double oldLongitude,
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedN,
                                                   final Speed oldSpeedE,
                                                   final Speed oldSpeedD,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX,
                angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14817
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14874
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final Angle oldLatitude,
                                                   final Angle oldLongitude,
                                                   final Distance oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight,
                oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15037
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15094
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final double oldLatitude,
                                                   final double oldLongitude,
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
File Line
com/irurueta/navigation/lateration/HomogeneousLinearLeastSquaresLateration3DSolver.java 177
com/irurueta/navigation/lateration/InhomogeneousLinearLeastSquaresLateration3DSolver.java 177
com/irurueta/navigation/lateration/NonLinearLeastSquaresLateration3DSolver.java 404
    private void internalSetSpheres(final Sphere[] spheres) {
        if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final Point3D[] positions = new Point3D[spheres.length];
        final double[] distances = new double[spheres.length];
        for (int i = 0; i < spheres.length; i++) {
            final Sphere sphere = spheres[i];
            positions[i] = sphere.getCenter();
            distances[i] = sphere.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }
File Line
com/irurueta/navigation/indoor/position/PROMedSRobustRssiPositionEstimator3D.java 370
com/irurueta/navigation/indoor/position/PROSACRobustMixedPositionEstimator2D.java 365
com/irurueta/navigation/indoor/position/PROSACRobustRangingAndRssiPositionEstimator2D.java 370
com/irurueta/navigation/indoor/position/PROSACRobustRangingPositionEstimator2D.java 366
com/irurueta/navigation/indoor/position/PROSACRobustRssiPositionEstimator2D.java 368
            final RobustRssiPositionEstimatorListener<Point3D> listener) {
        this(sources, fingerprint, listener);
        internalSetSourceQualityScores(sourceQualityScores);
        internalSetFingerprintReadingsQualityScores(fingerprintReadingQualityScores);
    }

    /**
     * Returns quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each radio source.
     */
    public double[] getSourceQualityScores() {
        return mSourceQualityScores;
    }

    /**
     * Sets quality scores corresponding to each radio source.
     * The larger the score value the better the quality of the radio source.
     *
     * @param sourceQualityScores quality scores corresponding to each radio source.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setSourceQualityScores(final double[] sourceQualityScores)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSourceQualityScores(sourceQualityScores);
    }

    /**
     * Gets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each reading within provided
     * fingerprint.
     */
    public double[] getFingerprintReadingsQualityScores() {
        return mFingerprintReadingsQualityScores;
    }

    /**
     * Sets quality scores corresponding to each reading within provided fingerprint.
     * The larger the score value the better the quality of the reading.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @param fingerprintReadingsQualityScores quality scores corresponding to each
     *                                         reading within provided fingerprint.
     * @throws LockedException          if this instance is locked.
     * @throws IllegalArgumentException if provided quality scores length is smaller
     *                                  than minimum required samples.
     */
    public void setFingerprintReadingsQualityScores(
            final double[] fingerprintReadingsQualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator.java 907
com/irurueta/navigation/indoor/radiosource/RangingAndRssiRadioSourceEstimator.java 857
        return areValidReadings(mReadings);
    }

    /**
     * Estimate position, transmitted power and path loss exponent.
     *
     * @throws RadioSourceEstimationException if estimation fails.
     * @throws NotReadyException              if estimator is not ready.
     * @throws LockedException                if estimator is locked.
     */
    @Override
    public void estimate() throws RadioSourceEstimationException, NotReadyException,
            LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mLocked = true;

            if (mListener != null) {
                mListener.onEstimateStart(this);
            }

            createInnerEstimatorsIfNeeded();

            final List<RangingReadingLocated<S, P>> rangingReadings = new ArrayList<>();
            final List<RssiReadingLocated<S, P>> rssiReadings = new ArrayList<>();
            for (final ReadingLocated<P> reading : mReadings) {
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 1974
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 2248
    public void copyFrom(final INSLooselyCoupledKalmanState input) {
        // copy coordinate transformation matrix
        if (input.mBodyToEcefCoordinateTransformationMatrix == null) {
            mBodyToEcefCoordinateTransformationMatrix = null;
        } else {
            if (mBodyToEcefCoordinateTransformationMatrix == null) {
                mBodyToEcefCoordinateTransformationMatrix =
                        new Matrix(input.mBodyToEcefCoordinateTransformationMatrix);
            } else {
                mBodyToEcefCoordinateTransformationMatrix.copyFrom(
                        input.mBodyToEcefCoordinateTransformationMatrix);
            }
        }

        mVx = input.mVx;
        mVy = input.mVy;
        mVz = input.mVz;

        mX = input.mX;
        mY = input.mY;
        mZ = input.mZ;

        mAccelerationBiasX = input.mAccelerationBiasX;
        mAccelerationBiasY = input.mAccelerationBiasY;
        mAccelerationBiasZ = input.mAccelerationBiasZ;

        mGyroBiasX = input.mGyroBiasX;
        mGyroBiasY = input.mGyroBiasY;
        mGyroBiasZ = input.mGyroBiasZ;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1295
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1406
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3315
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 821
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 930
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3845
        for (final FrameBodyKinematics measurement : mMeasurements) {
            final BodyKinematics measuredKinematics = measurement.getKinematics();
            final ECEFFrame ecefFrame = measurement.getFrame();
            final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
            final double timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame,
                    previousEcefFrame, expectedKinematics);

            final double fMeasX = measuredKinematics.getFx();
            final double fMeasY = measuredKinematics.getFy();
            final double fMeasZ = measuredKinematics.getFz();

            final double fTrueX = expectedKinematics.getFx();
            final double fTrueY = expectedKinematics.getFy();
            final double fTrueZ = expectedKinematics.getFz();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 6173
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1133
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 490
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    public Acceleration getInitialBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 756
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 831
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 221
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 982
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 294
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1055
                new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Matrix>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<Matrix> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final Matrix currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 840
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 217
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 848
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 914
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 294
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 926
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 765
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1887
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 225
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1917
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 838
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1962
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 301
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1992
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 206
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 983
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 294
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1057
                new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4183
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5117
            final boolean commonAxisUsed, final double[] bias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4367
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4932
            final boolean commonAxisUsed, final Matrix bias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 461
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 1889
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 1891
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3097
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3142
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solutions.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    public double getInitialBiasX() {
        return mInitialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    public double getInitialBiasY() {
        return mInitialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    public double getInitialBiasZ() {
        return mInitialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mInitialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    public Acceleration getInitialBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4388
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5351
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4580
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5158
            final boolean commonAxisUsed, final Matrix initialBias,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores,
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 691
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 676
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 766
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 751
                        new MSACRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation,
                            final int i) {
                        return computeError(mSequences.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustEasyGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2010
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2026
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2081
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2097
            final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mListener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    public int getMinimumRequiredMeasurements() {
        return mCommonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS :
                MINIMUM_MEASUREMENTS_GENERAL;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    public boolean isReady() {
        return mMeasurements != null && mMeasurements.size() >= getMinimumRequiredMeasurements()
                && mPosition != null && mYear != null;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Gets Earth's magnetic model.
     *
     * @return Earth's magnetic model or null if not provided.
     */
    public WorldMagneticModel getMagneticModel() {
        return mMagneticModel;
    }

    /**
     * Sets Earth's magnetic model.
     *
     * @param magneticModel Earth's magnetic model to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMagneticModel(final WorldMagneticModel magneticModel)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMagneticModel = magneticModel;
    }

    /**
     * Estimates magnetometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    public void calibrate() throws LockedException, NotReadyException,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 338
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1115
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 304
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1064
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 562
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2270
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 590
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2295
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 380
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1141
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 354
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1112
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 289
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1065
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 566
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2269
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 589
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2293
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 380
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1143
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4809
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5855
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5008
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5656
            final boolean commonAxisUsed, final Matrix hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4918
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5964
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5117
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5765
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        qualityScores, position, measurements, commonAxisUsed,
                        initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores,
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11431
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11565
                                    final double fz,
                                    final double angularRateX,
                                    final double angularRateY,
                                    final double angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException {
        try {
            navigateECEF(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(),
                    oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), fx, fy, fz,
                    angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECEF frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECEF(final Time timeInterval,
                                    final ECEFFrame oldFrame,
                                    final double fx,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11434
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11652
                                    final double angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException {
        try {
            navigateECEF(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(),
                    oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), fx, fy, fz,
                    angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECEF frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECEF(final Time timeInterval,
                                    final ECEFFrame oldFrame,
                                    final double fx,
                                    final double fy,
                                    final double fz,
                                    final double angularRateX,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11568
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11733
                                    final double angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException {
        try {
            navigateECEF(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(),
                    oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), fx, fy, fz,
                    angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECEF frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECEF(final Time timeInterval,
                                    final ECEFFrame oldFrame,
                                    final Acceleration fx,
                                    final Acceleration fy,
                                    final Acceleration fz,
                                    final double angularRateX,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11649
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11730
                                    final double fz,
                                    final AngularSpeed angularRateX,
                                    final AngularSpeed angularRateY,
                                    final AngularSpeed angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException {
        try {
            navigateECEF(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(),
                    oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), fx, fy, fz,
                    angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECEF frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECEF(final Time timeInterval,
                                    final ECEFFrame oldFrame,
                                    final double fx,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5275
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5409
                                   final double fz,
                                   final double angularRateX,
                                   final double angularRateY,
                                   final double angularRateZ,
                                   final ECIFrame result)
            throws InertialNavigatorException {
        try {
            navigateECI(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(),
                    oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), fx, fy, fz,
                    angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECI frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECI(final Time timeInterval,
                                   final ECIFrame oldFrame,
                                   final double fx,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5278
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5496
                                   final double angularRateZ,
                                   final ECIFrame result)
            throws InertialNavigatorException {
        try {
            navigateECI(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(),
                    oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), fx, fy, fz,
                    angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECI frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECI(final Time timeInterval,
                                   final ECIFrame oldFrame,
                                   final double fx,
                                   final double fy,
                                   final double fz,
                                   final double angularRateX,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5412
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5577
                                   final double angularRateZ,
                                   final ECIFrame result)
            throws InertialNavigatorException {
        try {
            navigateECI(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(),
                    oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), fx, fy, fz,
                    angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECI frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECI(final Time timeInterval,
                                   final ECIFrame oldFrame,
                                   final Acceleration fx,
                                   final Acceleration fy,
                                   final Acceleration fz,
                                   final double angularRateX,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5493
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5574
                                   final double fz,
                                   final AngularSpeed angularRateX,
                                   final AngularSpeed angularRateY,
                                   final AngularSpeed angularRateZ,
                                   final ECIFrame result)
            throws InertialNavigatorException {
        try {
            navigateECI(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(),
                    oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), fx, fy, fz,
                    angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECI frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECI(final Time timeInterval,
                                   final ECIFrame oldFrame,
                                   final double fx,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11320
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11462
                                   final double fz,
                                   final double angularRateX,
                                   final double angularRateY,
                                   final double angularRateZ,
                                   final NEDFrame result)
            throws InertialNavigatorException {
        try {
            navigateNED(timeInterval, oldFrame.getLatitude(), oldFrame.getLongitude(),
                    oldFrame.getHeight(), oldFrame.getCoordinateTransformation(),
                    oldFrame.getVn(), oldFrame.getVe(), oldFrame.getVd(),
                    fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous NED frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateNED(final Time timeInterval,
                                   final NEDFrame oldFrame,
                                   final double fx,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11323
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11553
                                   final double angularRateZ,
                                   final NEDFrame result)
            throws InertialNavigatorException {
        try {
            navigateNED(timeInterval, oldFrame.getLatitude(), oldFrame.getLongitude(),
                    oldFrame.getHeight(), oldFrame.getCoordinateTransformation(),
                    oldFrame.getVn(), oldFrame.getVe(), oldFrame.getVd(),
                    fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous NED frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateNED(final Time timeInterval,
                                   final NEDFrame oldFrame,
                                   final double fx,
                                   final double fy,
                                   final double fz,
                                   final double angularRateX,
File Line
com/irurueta/navigation/lateration/MSACRobustLateration2DSolver.java 233
com/irurueta/navigation/lateration/PROSACRobustLateration2DSolver.java 561
com/irurueta/navigation/lateration/RANSACRobustLateration2DSolver.java 311
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mDistances.length;
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices, final List<Point2D> solutions) {
                        solvePreliminarSolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final Point2D currentEstimation, final int i) {
                        return Math.abs(currentEstimation.distanceTo(mPositions[i]) - mDistances[i]);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustLateration2DSolver.this.isReady();
File Line
com/irurueta/navigation/gnss/GNSSKalmanFilteredEstimator.java 458
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1497
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java 1344
    public boolean getState(final GNSSKalmanState result) {
        if (mState != null) {
            result.copyFrom(mState);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets timestamp expressed in seconds since epoch time when Kalman filter state
     * was last propagated.
     *
     * @return timestamp expressed in seconds since epoch time when Kalman filter
     * state was last propagated.
     */
    public Double getLastStateTimestamp() {
        return mLastStateTimestamp;
    }

    /**
     * Gets timestamp since epoch time when Kalman filter state was last propageted.
     *
     * @param result instance where timestamp since epoch time when Kalman filter
     *               state was last propagated will be stored.
     * @return true if result instance is updated, false otherwise.
     */
    public boolean getLastStateTimestampAsTime(final Time result) {
        if (mLastStateTimestamp != null) {
            result.setValue(mLastStateTimestamp);
            result.setUnit(TimeUnit.SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets timestamp since epoch time when Kalman filter state was last propagated.
     *
     * @return timestamp since epoch time when Kalman filter state was last
     * propagated.
     */
    public Time getLastStateTimestampAsTime() {
        return mLastStateTimestamp != null ?
                new Time(mLastStateTimestamp, TimeUnit.SECOND) : null;
    }

    /**
     * Indicates whether this estimator is running or not.
     *
     * @return true if this estimator is running, false otherwise.
     */
    public boolean isRunning() {
        return mRunning;
    }

    /**
     * Indicates whether provided measurements are ready to
     * be used for an update.
     *
     * @param measurements measurements to be checked.
     * @return true if estimator is ready, false otherwise.
     */
    public static boolean isUpdateMeasurementsReady(
File Line
com/irurueta/navigation/indoor/position/NonLinearMixedPositionEstimator2D.java 325
com/irurueta/navigation/indoor/position/NonLinearMixedPositionEstimator3D.java 328
com/irurueta/navigation/indoor/position/NonLinearRangingAndRssiPositionEstimator2D.java 325
com/irurueta/navigation/indoor/position/NonLinearRangingAndRssiPositionEstimator3D.java 328
com/irurueta/navigation/indoor/position/NonLinearRangingPositionEstimator2D.java 328
com/irurueta/navigation/indoor/position/NonLinearRangingPositionEstimator3D.java 328
com/irurueta/navigation/indoor/position/NonLinearRssiPositionEstimator2D.java 329
com/irurueta/navigation/indoor/position/NonLinearRssiPositionEstimator3D.java 329
        Point2D[] positionsArray = new InhomogeneousPoint2D[size];
        positionsArray = positions.toArray(positionsArray);

        final double[] distancesArray = new double[size];
        final double[] distanceStandardDeviationsArray = new double[size];
        for (int i = 0; i < size; i++) {
            distancesArray[i] = distances.get(i);
            distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);
        }

        try {
            mTrilaterationSolver.setPositionsDistancesAndStandardDeviations(
                    positionsArray, distancesArray, distanceStandardDeviationsArray);
        } catch (final LockedException e) {
            throw new IllegalArgumentException(e);
        }
    }

    /**
     * Initializes lateration solver.
     */
    private void init() {
        mTrilaterationSolver = new NonLinearLeastSquaresLateration2DSolver(
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator.java 1031
com/irurueta/navigation/indoor/radiosource/RangingAndRssiRadioSourceEstimator.java 955
                    final Matrix rssiCov = mRssiInnerEstimator.getEstimatedCovariance();
                    if (mEstimatedPositionCovariance != null && rssiCov != null) {
                        final int dims = getNumberOfDimensions();
                        int n = dims;
                        if (mTransmittedPowerEstimationEnabled) {
                            n++;
                        }
                        if (mPathLossEstimationEnabled) {
                            n++;
                        }

                        final int dimsMinus1 = dims - 1;
                        final int nMinus1 = n - 1;
                        mEstimatedCovariance = new Matrix(n, n);
                        mEstimatedCovariance.setSubmatrix(0, 0,
                                dimsMinus1, dimsMinus1,
                                mEstimatedPositionCovariance);
                        mEstimatedCovariance.setSubmatrix(dims, dims,
                                nMinus1, nMinus1, rssiCov);
                    } else {
                        mEstimatedCovariance = null;
                    }
                }
File Line
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator2D.java 399
com/irurueta/navigation/indoor/radiosource/MixedRadioSourceEstimator3D.java 403
        final Point2D estimatedPosition = getEstimatedPosition();
        if (estimatedPosition == null) {
            return null;
        }

        final Matrix estimatedPositionCovariance = getEstimatedPositionCovariance();

        final Double transmittedPowerdBm = getEstimatedTransmittedPowerdBm();

        final Double transmittedPowerVariance = getEstimatedTransmittedPowerVariance();
        final Double transmittedPowerStandardDeviation = transmittedPowerVariance != null ?
                Math.sqrt(transmittedPowerVariance) : null;

        final double pathLossExponent = getEstimatedPathLossExponent();

        final Double pathLossExponentVariance = getEstimatedPathLossExponentVariance();
        final Double pathLossExponentStandardDeviation = pathLossExponentVariance != null ?
                Math.sqrt(pathLossExponentVariance) : null;

        if (source instanceof WifiAccessPoint) {
            final WifiAccessPoint accessPoint = (WifiAccessPoint) source;
            if (transmittedPowerdBm != null) {
                return new WifiAccessPointWithPowerAndLocated2D(accessPoint.getBssid(),
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 942
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 428
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 957
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 954
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Solution<Point3D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/SequentialRobustMixedRadioSourceEstimator.java 2161
com/irurueta/navigation/indoor/radiosource/SequentialRobustRangingAndRssiRadioSourceEstimator.java 2061
                    final Matrix rssiCov = mRssiEstimator.getCovariance();
                    if (mEstimatedPositionCovariance != null && rssiCov != null) {
                        final int dims = getNumberOfDimensions();
                        int n = dims;
                        if (mTransmittedPowerEstimationEnabled) {
                            n++;
                        }
                        if (mPathLossEstimationEnabled) {
                            n++;
                        }

                        final int dimsMinus1 = dims - 1;
                        final int nMinus1 = n - 1;
                        mCovariance = new Matrix(n, n);
                        mCovariance.setSubmatrix(0, 0,
                                dimsMinus1, dimsMinus1,
                                mEstimatedPositionCovariance);
                        mCovariance.setSubmatrix(dims, dims,
                                nMinus1, nMinus1, rssiCov);
                    } else {
                        mCovariance = null;
                    }
                }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1459
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1502
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2231
            b.setElementAtIndex(i, fMeasZ - fTrueZ - mBiasZ);
            i++;
        }

        final Matrix unknowns = Utils.solve(a, b);

        final double sx = unknowns.getElementAtIndex(0);
        final double sy = unknowns.getElementAtIndex(1);
        final double sz = unknowns.getElementAtIndex(2);
        final double mxy = unknowns.getElementAtIndex(3);
        final double mxz = unknowns.getElementAtIndex(4);
        final double myx = unknowns.getElementAtIndex(5);
        final double myz = unknowns.getElementAtIndex(6);
        final double mzx = unknowns.getElementAtIndex(7);
        final double mzy = unknowns.getElementAtIndex(8);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 856
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 904
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 981
            b.setElementAtIndex(i, fMeasZ - fTrueZ);
            i++;
        }

        final Matrix unknowns = Utils.solve(a, b);

        final double bx = unknowns.getElementAtIndex(0);
        final double by = unknowns.getElementAtIndex(1);
        final double bz = unknowns.getElementAtIndex(2);
        final double sx = unknowns.getElementAtIndex(3);
        final double sy = unknowns.getElementAtIndex(4);
        final double sz = unknowns.getElementAtIndex(5);
        final double mxy = unknowns.getElementAtIndex(6);
        final double mxz = unknowns.getElementAtIndex(7);
        final double myz = unknowns.getElementAtIndex(8);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7804
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5649
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5742
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2801
    private double evaluateGeneral(final double[] params) throws EvaluationException {
        final double bx = params[0];
        final double by = params[1];
        final double bz = params[2];

        final double m11 = params[3];
        final double m21 = params[4];
        final double m31 = params[5];

        final double m12 = params[6];
        final double m22 = params[7];
        final double m32 = params[8];

        final double m13 = params[9];
        final double m23 = params[10];
        final double m33 = params[11];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 957
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 338
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 968
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 815
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 886
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 800
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 2008
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 341
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 2039
            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            innerEstimator.setStopThreshold(mStopThreshold);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1882
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 549
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1901
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1553
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1784
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1521
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 4046
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 557
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 4109
            mInliersData = null;
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 5852
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 2677
                                  final Matrix preliminaryMa) {
        // We know that measured specific force is:
        // fmeas = ba + (I + Ma) * ftrue

        // Hence:
        //  [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        //  [fmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [ftruey]
        //  [fmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [ftruez]

        final BodyKinematics measuredKinematics = measurement.getKinematics();
        final ECEFFrame ecefFrame = measurement.getFrame();
        final ECEFFrame previousEcefFrame = measurement.getPreviousFrame();
        final double timeInterval = measurement.getTimeInterval();

        final BodyKinematics expectedKinematics = ECEFKinematicsEstimator
                .estimateKinematicsAndReturnNew(timeInterval, ecefFrame,
                        previousEcefFrame);

        final double fMeasX1 = measuredKinematics.getFx();
        final double fMeasY1 = measuredKinematics.getFy();
        final double fMeasZ1 = measuredKinematics.getFz();

        final double fTrueX = expectedKinematics.getFx();
        final double fTrueY = expectedKinematics.getFy();
        final double fTrueZ = expectedKinematics.getFz();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4087
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3757
        final double m22 = params[5];

        final double m13 = params[6];
        final double m23 = params[7];
        final double m33 = params[8];

        final double g11 = params[9];
        final double g21 = params[10];
        final double g31 = params[11];

        final double g12 = params[12];
        final double g22 = params[13];
        final double g32 = params[14];

        final double g13 = params[15];
        final double g23 = params[16];
        final double g33 = params[17];

        return evaluate(i, bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3955
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5030
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3749
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4824
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 7122
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 7234
            if (mIdentity == null) {
                mIdentity = Matrix.identity(
                        BodyMagneticFluxDensity.COMPONENTS,
                        BodyMagneticFluxDensity.COMPONENTS);
            }

            if (mTmp1 == null) {
                mTmp1 = new Matrix(BodyMagneticFluxDensity.COMPONENTS,
                        BodyMagneticFluxDensity.COMPONENTS);
            }

            if (mTmp2 == null) {
                mTmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (mTmp3 == null) {
                mTmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            if (mTmp4 == null) {
                mTmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            mIdentity.add(preliminaryResult, mTmp1);
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 7937
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 7992
    public static void navigateECEF(final double timeInterval,
                                    final ECEFPosition oldPosition,
                                    final CoordinateTransformation oldC,
                                    final double oldVx,
                                    final double oldVy,
                                    final double oldVz,
                                    final double fx,
                                    final double fy,
                                    final double fz,
                                    final double angularRateX,
                                    final double angularRateY,
                                    final double angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8047
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8102
    public static void navigateECEF(final double timeInterval,
                                    final double oldX,
                                    final double oldY,
                                    final double oldZ,
                                    final CoordinateTransformation oldC,
                                    final ECEFVelocity oldVelocity,
                                    final double fx,
                                    final double fy,
                                    final double fz,
                                    final double angularRateX,
                                    final double angularRateY,
                                    final double angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8524
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8579
    public static void navigateECEF(final double timeInterval,
                                    final Point3D oldPosition,
                                    final CoordinateTransformation oldC,
                                    final double oldVx,
                                    final double oldVy,
                                    final double oldVz,
                                    final double fx,
                                    final double fy,
                                    final double fz,
                                    final double angularRateX,
                                    final double angularRateY,
                                    final double angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getInhomX(), oldPosition.getInhomY(), oldPosition.getInhomZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8980
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9035
    public static void navigateECEF(final double timeInterval,
                                    final Distance oldX,
                                    final Distance oldY,
                                    final Distance oldZ,
                                    final CoordinateTransformation oldC,
                                    final ECEFVelocity oldVelocity,
                                    final double fx,
                                    final double fy,
                                    final double fz,
                                    final double angularRateX,
                                    final double angularRateY,
                                    final double angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9079
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9121
    public static void navigateECEF(final double timeInterval,
                                    final Distance oldX,
                                    final Distance oldY,
                                    final Distance oldZ,
                                    final CoordinateTransformation oldC,
                                    final double oldVx,
                                    final double oldVy,
                                    final double oldVz,
                                    final BodyKinematics kinematics,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                kinematics.getFx(), kinematics.getFy(), kinematics.getFz(),
                kinematics.getAngularRateX(), kinematics.getAngularRateY(),
                kinematics.getAngularRateZ(), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9367
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9422
    public static void navigateECEF(final double timeInterval,
                                    final ECEFPosition oldPosition,
                                    final CoordinateTransformation oldC,
                                    final Speed oldSpeedX,
                                    final Speed oldSpeedY,
                                    final Speed oldSpeedZ,
                                    final double fx,
                                    final double fy,
                                    final double fz,
                                    final double angularRateX,
                                    final double angularRateY,
                                    final double angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9742
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9794
    public static void navigateECEF(final double timeInterval,
                                    final ECEFPosition oldPosition,
                                    final CoordinateTransformation oldC,
                                    final double oldVx,
                                    final double oldVy,
                                    final double oldVz,
                                    final Acceleration fx,
                                    final Acceleration fy,
                                    final Acceleration fz,
                                    final double angularRateX,
                                    final double angularRateY,
                                    final double angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10163
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10215
    public static void navigateECEF(final double timeInterval,
                                    final ECEFPosition oldPosition,
                                    final CoordinateTransformation oldC,
                                    final double oldVx,
                                    final double oldVy,
                                    final double oldVz,
                                    final double fx,
                                    final double fy,
                                    final double fz,
                                    final AngularSpeed angularRateX,
                                    final AngularSpeed angularRateY,
                                    final AngularSpeed angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 3995
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4051
    public static void navigateECI(final double timeInterval,
                                   final Point3D oldPosition,
                                   final CoordinateTransformation oldC,
                                   final double oldVx,
                                   final double oldVy,
                                   final double oldVz,
                                   final double fx,
                                   final double fy,
                                   final double fz,
                                   final double angularRateX,
                                   final double angularRateY,
                                   final double angularRateZ,
                                   final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldPosition.getInhomX(), oldPosition.getInhomY(), oldPosition.getInhomZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECI(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4292
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4335
    public static void navigateECI(final double timeInterval,
                                   final Distance oldX,
                                   final Distance oldY,
                                   final Distance oldZ,
                                   final CoordinateTransformation oldC,
                                   final double oldVx,
                                   final double oldVy,
                                   final double oldVz,
                                   final BodyKinematics kinematics,
                                   final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                kinematics.getFx(), kinematics.getFy(), kinematics.getFz(),
                kinematics.getAngularRateX(), kinematics.getAngularRateY(),
                kinematics.getAngularRateZ(), result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECI(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7804
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4027
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5742
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2801
    private double evaluateGeneral(final double[] params) throws EvaluationException {
        final double bx = params[0];
        final double by = params[1];
        final double bz = params[2];

        final double m11 = params[3];
        final double m21 = params[4];
        final double m31 = params[5];

        final double m12 = params[6];
        final double m22 = params[7];
        final double m32 = params[8];

        final double m13 = params[9];
        final double m23 = params[10];
        final double m33 = params[11];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7905
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5641
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5889
            mFmeas.setElementAtIndex(2, mFmeasZ);

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);

            mB.setElementAtIndex(0, bx);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 878
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 916
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 295
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 926
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1887
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 554
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1906
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1909
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 582
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1933
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 997
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 376
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 1008
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 843
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1963
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 303
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1995
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1789
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 4051
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 562
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 4114
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1812
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 4075
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 589
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 4140
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 920
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 2044
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 383
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 2074
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 962
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 343
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 973
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 832
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1778
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1805
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 909
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 891
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 2013
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 346
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 2044
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            mRunning = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMedS;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3359
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3633
            final double[] bias, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param bias   known accelerometer bias.
     * @param method robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final Matrix bias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 381
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 375
    private Matrix mEstimatedMg;

    /**
     * Estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     */
    private Matrix mEstimatedGg;

    /**
     * Estimated covariance matrix for estimated parameters.
     */
    private Matrix mEstimatedCovariance;

    /**
     * Estimated chi square value.
     */
    private double mEstimatedChiSq;

    /**
     * Indicates whether calibrator is running.
     */
    private boolean mRunning;

    /**
     * Acceleration fixer.
     */
    private final AccelerationFixer mAccelerationFixer =
            new AccelerationFixer();

    /**
     * Index of current point being evaluated.
     */
    private int mI;

    /**
     * Holds integrated rotation of a sequence.
     */
    private final Quaternion mQ = new Quaternion();

    /**
     * Contains a copy of input sequences where fixed body kinematics will be updated.
     */
    private List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> mFixedSequences;

    /**
     * Contains measured specific force on a sample within a sequence
     * expressed as a 3x1 matrix.
     */
    private Matrix mMeasuredSpecificForce;

    /**
     * Contains fixed specific force for a sample within a sequence
     * using provided accelerometer parameters and expressed as a 3x1
     * matrix.
     */
    private Matrix mTrueSpecificForce;

    /**
     * Contains measured angular rate on a sample within a sequence
     * expressed as a 3x1 matrix.
     */
    private Matrix mMeasuredAngularRate;

    /**
     * Contains fixed specific force for a samplewithin a sequence
     * using current parameters being estimated.
     */
    private Matrix mTrueAngularRate;

    /**
     * Internally holds cross-coupling errors during calibration.
     */
    private Matrix mM;

    /**
     * Internally holds inverse of cross-coupling errors during calibration.
     */
    private Matrix mInvM;

    /**
     * Internally holds biases during calibration.
     */
    private Matrix mB;

    /**
     * Internally hold g-dependent cross biases during calibration.
     */
    private Matrix mG;

    /**
     * Internally holds angular rate bias due to g-dependent cross biases
     */
    private Matrix mTmp;

    /**
     * Holds normalized gravity versor at the start of a sequence.
     * This is used to compute rotations of gravity versor during
     * calibration.
     */
    private final InhomogeneousPoint3D mStartPoint = new InhomogeneousPoint3D();

    /**
     * Holds normalized gravity versor at the end of a sequence.
     * This is used to compute rotations of gravity versor during
     * calibration.
     */
    private final InhomogeneousPoint3D mEndPoint = new InhomogeneousPoint3D();

    /**
     * Contains expected gravity versor obtained from measurements fixed
     * using known accelerometer parameters.
     * This is reused for memory efficiency purposes during calibration.
     */
    private final InhomogeneousPoint3D mExpectedEndPoint = new InhomogeneousPoint3D();

    /**
     * Array containing normalized gravity versor before (former 3 values)
     * and after (latter 3 values) a given sequence.
     * This is used during calibration.
     */
    private double[] mPoint;

    /**
     * Constructor.
     */
    public EasyGyroscopeCalibrator() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4123
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5650
            final int i, final double[] params)
            throws EvaluationException {

        final double bx = params[0];
        final double by = params[1];
        final double bz = params[2];

        final double m11 = params[3];
        final double m21 = params[4];
        final double m31 = params[5];

        final double m12 = params[6];
        final double m22 = params[7];
        final double m32 = params[8];

        final double m13 = params[9];
        final double m23 = params[10];
        final double m33 = params[11];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3712
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5614
        if (mEstimatedMg == null) {
            mEstimatedMg = m;
        } else {
            mEstimatedMg.copyFrom(m);
        }

        for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
            mEstimatedMg.setElementAt(i, i,
                    mEstimatedMg.getElementAt(i, i) - 1.0);
        }

        if (mEstimatedGg == null) {
            mEstimatedGg = new Matrix(
                    BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            mEstimatedGg.initialize(0.0);
        }

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Computes gravity versor error at the end of a sequence using provided
     * parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param i      row position.
     * @param params array containing parameters for the general purpose case
     *               when G-dependent cross biases are taken into account. Must
     *               have length 18.
     * @return error between estimated and measured gravity versor.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneralWithGDependentCrossBiases(
            final int i, final double[] params)
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5541
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5993
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2906
            mFmeas.setElementAtIndex(2, mFmeasZ);

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);

            mB.setElementAtIndex(0, mBiasX);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3711
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4052
                System.arraycopy(buffer, 0, initial, 9, num);

                return initial;
            }

            @Override
            public void evaluate(final int i, final double[] point,
                                 final double[] result, final double[] params,
                                 final Matrix jacobian) {
                // We know that:
                // Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
                // Ωmeasy = by + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
                // Ωmeasz = bz + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

                // Hence, the derivatives respect the parameters bx, by, bz,
                // sx, sy, sz, mxy mxz, myz, g11, g12, g13, g21, g22, g23,
                // g31, g32, and g33 is:

                // d(Ωmeasx)/d(bx) = 1.0
                // d(Ωmeasx)/d(by) = 0.0
                // d(Ωmeasx)/d(bz) = 0.0
                // d(Ωmeasx)/d(sx) = Ωtruex
                // d(Ωmeasx)/d(sy) = 0.0
                // d(Ωmeasx)/d(sz) = 0.0
                // d(Ωmeasx)/d(mxy) = Ωtruey
                // d(Ωmeasx)/d(mxz) = Ωtruez
                // d(Ωmeasx)/d(myz) = 0.0
                // d(Ωmeasx)/d(g11) = ftruex
                // d(Ωmeasx)/d(g12) = ftruey
                // d(Ωmeasx)/d(g13) = ftruez
                // d(Ωmeasx)/d(g21) = 0.0
                // d(Ωmeasx)/d(g22) = 0.0
                // d(Ωmeasx)/d(g23) = 0.0
                // d(Ωmeasx)/d(g31) = 0.0
                // d(Ωmeasx)/d(g32) = 0.0
                // d(Ωmeasx)/d(g33) = 0.0

                // d(Ωmeasy)/d(bx) = 0.0
                // d(Ωmeasy)/d(by) = 1.0
                // d(Ωmeasy)/d(bz) = 0.0
                // d(Ωmeasy)/d(sx) = 0.0
                // d(Ωmeasy)/d(sy) = Ωtruey
                // d(Ωmeasy)/d(sz) = 0.0
                // d(Ωmeasy)/d(mxy) = 0.0
                // d(Ωmeasy)/d(mxz) = 0.0
                // d(Ωmeasy)/d(myz) = Ωtruez
                // d(Ωmeasx)/d(g11) = 0.0
                // d(Ωmeasx)/d(g12) = 0.0
                // d(Ωmeasx)/d(g13) = 0.0
                // d(Ωmeasx)/d(g21) = ftruex
                // d(Ωmeasx)/d(g22) = ftruey
                // d(Ωmeasx)/d(g23) = ftruez
                // d(Ωmeasx)/d(g31) = 0.0
                // d(Ωmeasx)/d(g32) = 0.0
                // d(Ωmeasx)/d(g33) = 0.0

                // d(Ωmeasz)/d(bx) = 0.0
                // d(Ωmeasz)/d(by) = 0.0
                // d(Ωmeasz)/d(bz) = 1.0
                // d(Ωmeasz)/d(sx) = 0.0
                // d(Ωmeasz)/d(sy) = 0.0
                // d(Ωmeasz)/d(sz) = Ωtruez
                // d(Ωmeasz)/d(mxy) = 0.0
                // d(Ωmeasz)/d(mxz) = 0.0
                // d(Ωmeasz)/d(myz) = 0.0
                // d(Ωmeasx)/d(g11) = 0.0
                // d(Ωmeasx)/d(g12) = 0.0
                // d(Ωmeasx)/d(g13) = 0.0
                // d(Ωmeasx)/d(g21) = 0.0
                // d(Ωmeasx)/d(g22) = 0.0
                // d(Ωmeasx)/d(g23) = 0.0
                // d(Ωmeasx)/d(g31) = ftruex
                // d(Ωmeasx)/d(g32) = ftruey
                // d(Ωmeasx)/d(g33) = ftruez

                final double bx = params[0];
                final double by = params[1];
                final double bz = params[2];

                final double sx = params[3];
                final double sy = params[4];
                final double sz = params[5];

                final double mxy = params[6];
                final double mxz = params[7];
                final double myz = params[8];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3434
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3708
            final double[] bias, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, bias, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, bias, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param bias   known gyroscope bias.
     * @param method robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final Matrix bias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2394
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2446
    public void navigate(final double timeInterval,
                         final ECEFPosition oldPosition,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final double fx,
                         final double fy,
                         final double fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/INSGNSSLooselyCoupledKalmanFilteredEstimator.java 1665
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1616
                timestamp - lastStateTimestamp <= mInsEstimator.getEpochInterval()) {
            return false;
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onUpdateGNSSMeasurementsStart(this);
            }

            mMeasurements = new ArrayList<>(measurements);

            mLsEstimator.setMeasurements(mMeasurements);
            mLsEstimator.setPriorPositionAndVelocityFromEstimation(mEstimation);
            if (mEstimation != null) {
                mLsEstimator.estimate(mEstimation);
            } else {
                mEstimation = mLsEstimator.estimate();
            }

            if (mListener != null) {
                mListener.onUpdateGNSSMeasurementsEnd(this);
            }
        } catch (final GNSSException e) {
            throw new INSGNSSException(e);
        } finally {
            mRunning = false;
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 262
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 275
            final KnownFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
        this(measurements, commonAxisUsed);
        mListener = listener;
    }

    /**
     * Gets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<? extends FrameBodyKinematics> getMeasurements() {
        return mMeasurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body kinematics measurements taken at different
     *                     frames (positions, orientations and velocities).
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final Collection<? extends FrameBodyKinematics> measurements)
            throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        mMeasurements = measurements;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return mCommonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }

        mCommonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    @Override
    public KnownFrameAccelerometerLinearLeastSquaresCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 731
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 737
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 707
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 712
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<Matrix> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2592
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2898
            final double biasX, final double biasY, final double biasZ,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param measurements list of body kinematics measurements with standard
     *                     deviations taken at different frames (positions, orientations
     *                     and velocities).
     * @param biasX        known x coordinate of accelerometer bias expressed in meters per
     *                     squared second (m/s^2).
     * @param biasY        known y coordinate of accelerometer bias expressed in meters per
     *                     squared second (m/s^2).
     * @param biasZ        known z coordinate of accelerometer bias expressed in meters per
     *                     squared second (m/s^2).
     * @param listener     listener to handle events raised by this calibrator.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4087
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5336
        final double m22 = params[5];

        final double m13 = params[6];
        final double m23 = params[7];
        final double m33 = params[8];

        final double g11 = params[9];
        final double g21 = params[10];
        final double g31 = params[11];

        final double g12 = params[12];
        final double g22 = params[13];
        final double g32 = params[14];

        final double g13 = params[15];
        final double g23 = params[16];
        final double g33 = params[17];

        return evaluate(i, bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4266
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5543
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5643
            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);

            mB.setElementAtIndex(0, bx);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3757
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5707
        final double m32 = params[5];

        final double m13 = params[6];
        final double m23 = params[7];
        final double m33 = params[8];

        final double g11 = params[9];
        final double g21 = params[10];
        final double g31 = params[11];

        final double g12 = params[12];
        final double g22 = params[13];
        final double g32 = params[14];

        final double g13 = params[15];
        final double g23 = params[16];
        final double g33 = params[17];

        return evaluate(i, m11, m21, m31, m12, m22, m32,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 816
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 801
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 334
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1111
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            innerEstimator.setStopThreshold(mStopThreshold);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1554
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1522
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 562
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2265
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3717
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4759
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3833
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4891
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2663
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2971
            final double biasX, final double biasY, final double biasZ,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ);
        }
    }

    /**
     * Creaates a robust gyroscope calibrator.
     *
     * @param measurements list of body kinematics measurements with standard
     *                     deviations taken at different frames (positions, orientations
     *                     and velocities).
     * @param biasX        known x coordinate of gyroscope bias expressed in radians per
     *                     second (rad/s).
     * @param biasY        known y coordinate of gyroscope bias expressed in radians per
     *                     second (rad/s).
     * @param biasZ        known z coordinate of gyroscope bias expressed in radians per
     *                     second (rad/s).
     * @param listener     listener to handle events raised by this calibrator.
     * @param method       robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3511
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4553
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, initialBias, initialMg, initialGg,
                        accelerometerBias, accelerometerMa);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3627
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4685
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases,
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2394
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10163
    public void navigate(final double timeInterval,
                         final ECEFPosition oldPosition,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final double fx,
                         final double fy,
                         final double fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2446
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10215
    public void navigate(final Time timeInterval,
                         final ECEFPosition oldPosition,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final double fx,
                         final double fy,
                         final double fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final double timeInterval,
File Line
com/irurueta/navigation/gnss/GNSSKalmanEpochEstimator.java 366
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanEpochEstimator.java 350
            h.setElementAt(j2, 7, 1.0);
        }

        // 6. Set-up measurement noise covariance matrix assuming all measurements are independent
        // and have equal variance for a given measurement type
        final double pseudoRangeSD = config.getPseudoRangeSD();
        final double pseudoRangeSD2 = pseudoRangeSD * pseudoRangeSD;
        final double rangeRateSD = config.getRangeRateSD();
        final double rangeRateSD2 = rangeRateSD * rangeRateSD;
        final Matrix r = new Matrix(2 * numberOfMeasurements, 2 * numberOfMeasurements);
        for (int i1 = 0, i2 = numberOfMeasurements; i1 < numberOfMeasurements; i1++, i2++) {
            r.setElementAt(i1, i1, pseudoRangeSD2);
            r.setElementAt(i2, i2, rangeRateSD2);
        }

        // 7. Calculate Kalman gain using (3.21)
        final Matrix hTransposed = h.transposeAndReturnNew();
        final Matrix tmp8 = h.multiplyAndReturnNew(
File Line
com/irurueta/navigation/indoor/Utils.java 264
com/irurueta/navigation/indoor/Utils.java 802
com/irurueta/navigation/indoor/Utils.java 1968
com/irurueta/navigation/indoor/Utils.java 2391
            final Double fingerprintRssiVariance,
            final Double pathLossExponentVariance,
            final Matrix fingerprintPositionCovariance,
            final Matrix radioSourcePositionCovariance,
            final Matrix estimatedPositionCovariance) throws IndoorException {

        if (fingerprintPosition == null || radioSourcePosition == null ||
                estimatedPosition == null) {
            return null;
        }

        //1st order Taylor expression of received power in 2D:
        //Pr(pi) = Pr(p1)
        //  - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
        //  - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
        //where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2

        final double x1 = fingerprintPosition.getInhomX();
        final double y1 = fingerprintPosition.getInhomY();

        final double xa = radioSourcePosition.getInhomX();
        final double ya = radioSourcePosition.getInhomY();

        final double xi = estimatedPosition.getInhomX();
        final double yi = estimatedPosition.getInhomY();

        final double[] mean = new double[]{
File Line
com/irurueta/navigation/indoor/Utils.java 460
com/irurueta/navigation/indoor/Utils.java 1160
                    jacobian.setElementAtIndex(0, derivativeFingerprintRssi);
                    jacobian.setElementAtIndex(1, derivativePathLossExponent);
                    jacobian.setElementAtIndex(2, derivativeX1);
                    jacobian.setElementAtIndex(3, derivativeY1);
                    jacobian.setElementAtIndex(4, derivativeXa);
                    jacobian.setElementAtIndex(5, derivativeYa);
                    jacobian.setElementAtIndex(6, derivativeXi);
                    jacobian.setElementAtIndex(7, derivativeYi);
                }

                @Override
                public int getNumberOfVariables() {
                    return 1;
                }
            }, mean, covariance);
        } catch (final AlgebraException | StatisticsException e) {
            throw new IndoorException(e);
        }
    }

    /**
     * Propagates provided variances (fingerprint rssi variance, path-loss exponent variance,
     * fingerprint position covariance and radio source position covariance) into
     * rssi variance by considering the 3D 1st order Taylor expression of received power.
     * Notice that any unknown variance is assumed to be zero.
     *
     * @param fingerprintRssi               closest located fingerprint reading RSSI expressed in dBm's.
     * @param pathLossExponent              path-loss exponent.
     * @param fingerprintPosition           position of closest fingerprint.
     * @param radioSourcePosition           radio source position associated to fingerprint reading.
     * @param estimatedPosition             position to be estimated. Usually this is equal to the
     *                                      initial position used by a non linear algorithm.
     * @param fingerprintRssiVariance       variance of fingerprint RSSI or null if unknown.
     * @param pathLossExponentVariance      variance of path-loss exponent or null if unknown.
     * @param fingerprintPositionCovariance covariance of fingerprint position or null if
     *                                      unknown.
     * @param radioSourcePositionCovariance covariance of radio source position or null
     *                                      if unknown.
     * @param estimatedPositionCovariance   covariance of position to be estimated or null
     *                                      if unknown. (This is usually unknown).
     * @return a normal distribution containing expected received RSSI value and its variance.
     * @throws IndoorException if something fails.
     */
    public static MultivariateNormalDist propagateVariancesToRssiVarianceFirstOrderNonLinear3D(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2687
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2954
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3067
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3213
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3700
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3659
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException |
                com.irurueta.numerical.NotReadyException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3043
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4021
                jacobian.setElementAt(2, 2, ftruez);
                jacobian.setElementAt(2, 3, 0.0);
                jacobian.setElementAt(2, 4, 0.0);
                jacobian.setElementAt(2, 5, 0.0);
            }
        });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double sx = result[0];
        final double sy = result[1];
        final double sz = result[2];

        final double mxy = result[3];
        final double mxz = result[4];
        final double myz = result[5];

        if (mEstimatedMa == null) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 7239
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2735
            mFmeas.setElementAtIndex(2, mFmeasZ);

            mM.setElementAt(0, 0, m11);
            mM.setElementAt(1, 0, m21);
            mM.setElementAt(2, 0, m31);

            mM.setElementAt(0, 1, m12);
            mM.setElementAt(1, 1, m22);
            mM.setElementAt(2, 1, m32);

            mM.setElementAt(0, 2, m13);
            mM.setElementAt(1, 2, m23);
            mM.setElementAt(2, 2, m33);

            Utils.inverse(mM, mInvM);

            // b = M^-1*ba
            mInvM.multiply(mBa, mB);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 874
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 350
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1108
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            innerEstimator.setStopThreshold(mStopThreshold);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 958
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 339
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 969
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 887
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 2009
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 342
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 2040
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 334
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1111
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            innerEstimator.setStopThreshold(mStopThreshold);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1774
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 558
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2266
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final Matrix preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1883
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 550
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1902
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1785
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 4047
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 558
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 4110
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 562
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2265
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3317
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3257
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4748
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4805
        final double m22 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final double g11 = result[9];
        final double g21 = result[10];
        final double g31 = result[11];

        final double g12 = result[12];
        final double g22 = result[13];
        final double g32 = result[14];

        final double g13 = result[15];
        final double g23 = result[16];
        final double g33 = result[17];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3591
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5127
                        return evaluateCommonAxis(mI, params);
                    }
                });

        final Matrix initialM = Matrix.identity(
                BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        initialM.add(getInitialMg());

        // Force initial M to be upper diagonal
        initialM.setElementAt(1, 0, 0.0);
        initialM.setElementAt(2, 0, 0.0);
        initialM.setElementAt(2, 1, 0.0);

        final Matrix invInitialM = Utils.inverse(initialM);
        final Matrix initialBg = getInitialBiasAsMatrix();
        final Matrix initialB = invInitialM.multiplyAndReturnNew(initialBg);

        mFitter.setFunctionEvaluator(
                new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
                    @Override
                    public int getNumberOfDimensions() {
                        // Before and after normalized gravity versors
                        return 2 * BodyKinematics.COMPONENTS;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3652
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2705
                        return evaluateCommonAxis(i, params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];

        final double m12 = result[4];
        final double m22 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final Matrix b = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3530
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2411
                        return evaluateGeneral(i, params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double m11 = result[0];
        final double m21 = result[1];
        final double m31 = result[2];

        final double m12 = result[3];
        final double m22 = result[4];
        final double m32 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final Matrix m = new Matrix(BodyKinematics.COMPONENTS,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 3714
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 7812
            final Matrix skewAlpha = Utils.skewMatrix(alphaIbb);

            // Obtain coordinate transformation matrix from the new attitude to the old
            // using Rodrigues' formula, (5.73)
            final Matrix cNewOld = Matrix.identity(ROWS, ROWS);
            if (magAlpha > ALPHA_THRESHOLD) {
                final double magAlpha2 = magAlpha * magAlpha;
                final double value1 = Math.sin(magAlpha) / magAlpha;
                final double value2 = (1.0 - Math.cos(magAlpha)) / magAlpha2;

                final Matrix tmp1 = skewAlpha.multiplyByScalarAndReturnNew(value1);
                final Matrix tmp2 = skewAlpha.multiplyByScalarAndReturnNew(value2);
                tmp2.multiply(skewAlpha);

                cNewOld.add(tmp1);
                cNewOld.add(tmp2);
            } else {
                cNewOld.add(skewAlpha);
            }
File Line
com/irurueta/navigation/gnss/ECEFPositionAndVelocity.java 344
com/irurueta/navigation/gnss/GNSSEstimation.java 255
    public ECEFPositionAndVelocity(final ECEFPositionAndVelocity input) {
        copyFrom(input);
    }

    /**
     * Gets cartesian x coordinate of position resolved in ECEF axes and
     * expressed in meters (m).
     *
     * @return cartesian x coordinate of position resolved in ECEF axes
     * and expressed in meters (m).
     */
    public double getX() {
        return mX;
    }

    /**
     * Sets cartesian x coordinate of position resolved in ECEF axes and
     * expressed in meters (m).
     *
     * @param x cartesian x coordinate of position resolved in ECEF axes
     *          and expressed in meters (m).
     */
    public void setX(final double x) {
        mX = x;
    }

    /**
     * Gets cartesian y coordinate of position resolved in ECEF axes and
     * expressed in meters (m).
     *
     * @return cartesian y coordinate of position resolved in ECEF axes
     * and expressed in meters (m).
     */
    public double getY() {
        return mY;
    }

    /**
     * Sets cartesian y coordinate of position resolved in ECEF axes and
     * expressed in meters (m).
     *
     * @param y cartesian y coordinate of position resolved in ECEF axes
     *          and expressed in meters (m).
     */
    public void setY(final double y) {
        mY = y;
    }

    /**
     * Gets cartesian z coordinate of position resolved in ECEF axes and
     * expressed in meters (m).
     *
     * @return cartesian z coordinate of position resolved in ECEF axes
     * and expressed in meters (m).
     */
    public double getZ() {
        return mZ;
    }

    /**
     * Sets cartesian z coordinate of position resolved in ECEF axes and
     * expressed in meters (m).
     *
     * @param z cartesian z coordinate of position resolved in ECEF axes
     *          and expressed in meters (m).
     */
    public void setZ(final double z) {
        mZ = z;
    }

    /**
     * Sets ECEF position expressed in meters (m).
     *
     * @param x cartesian x coordinate of position.
     * @param y cartesian y coordinate of position.
     * @param z cartesian z coordinate of position.
     */
    public void setPositionCoordinates(final double x, final double y, final double z) {
        mX = x;
        mY = y;
        mZ = z;
    }

    /**
     * Gets cartesian x coordinate of position resolved in ECEF axes.
     *
     * @param result instance where cartesian x coordinate of position will
     *               be stored.
     */
    public void getXDistance(final Distance result) {
File Line
com/irurueta/navigation/indoor/position/LinearMixedPositionEstimator.java 166
com/irurueta/navigation/indoor/position/LinearRangingAndRssiPositionEstimator.java 167
com/irurueta/navigation/indoor/position/LinearRangingPositionEstimator.java 166
com/irurueta/navigation/indoor/position/LinearRssiPositionEstimator.java 169
        } catch (LaterationException e) {
            throw new PositionEstimationException(e);
        }
    }

    /**
     * Gets known positions of radio sources used internally to solve lateration.
     *
     * @return known positions used internally.
     */
    @Override
    public P[] getPositions() {
        return mUseHomogeneousLinearSolver ?
                mHomogeneousTrilaterationSolver.getPositions() :
                mInhomogeneousTrilaterationSolver.getPositions();
    }

    /**
     * Gets euclidean distances from known located radio sources to
     * the location of provided readings in a fingerprint.
     * Distance values are used internally to solve lateration.
     *
     * @return euclidean distances used internally.
     */
    @Override
    public double[] getDistances() {
        return mUseHomogeneousLinearSolver ?
                mHomogeneousTrilaterationSolver.getDistances() :
                mInhomogeneousTrilaterationSolver.getDistances();
    }

    /**
     * Internally sets located radio sources used for lateration.
     *
     * @param sources located radio sources used for lateration.
     * @throws IllegalArgumentException if provided value is null or the number of
     *                                  provided sources is less than the required
     *                                  minimum.
     */
    protected void internalSetSources(final List<? extends RadioSourceLocated<P>> sources) {
        super.internalSetSources(sources);
        buildPositionsAndDistances();
    }

    /**
     * Internally sets fingerprint containing readings at an unknown location for provided
     * located radio sources.
     *
     * @param fingerprint fingerprint containing readings at an unknown location for
     *                    provided located radio sources.
     * @throws IllegalArgumentException if provided value is null.
     */
    protected void internalSetFingerprint(
            final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint) {
File Line
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 957
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 459
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator3D.java 459
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator2D.java 224
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator3D.java 224
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator2D.java 470
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 468
    public void setComputeAndKeepInliersEnabled(boolean computeAndKeepInliers)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResidualsEnabled() {
        return mComputeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        mComputeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final PROSACRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1394
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java 1269
    }

    /**
     * Gets last provided user kinematics containing applied specific force and
     * angular rates resolved in body axes.
     *
     * @return last provided user kinematics.
     */
    public BodyKinematics getKinematics() {
        return mKinematics != null ? new BodyKinematics(mKinematics) : null;
    }

    /**
     * Gets last provided user kinematics containing applied specific force and
     * angular rates resolved in body axes.
     *
     * @param result instance where last provided body kinematics will be stored.
     * @return true if provided result instance was updated, false otherwise.
     */
    public boolean getKinematics(final BodyKinematics result) {
        if (mKinematics != null) {
            result.copyFrom(mKinematics);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets corrected kinematics which are the last provided user kinematics after
     * removal of the biases estimated by the Kalman filter.
     *
     * @return corrected kinematics.
     * @see #getKinematics()
     */
    public BodyKinematics getCorrectedKinematics() {
        return mCorrectedKinematics != null ?
                new BodyKinematics(mCorrectedKinematics) : null;
    }

    /**
     * Gets corrected kinematics which are the last provided user kinematics after
     * removal of the biases estimated by the Kalman filter.
     *
     * @param result instance where corrected body kinematics will be stored.
     * @return true if provided result instance was updated, false otherwise.
     */
    public boolean getCorrectedKinematics(final BodyKinematics result) {
        if (mCorrectedKinematics != null) {
            result.copyFrom(mCorrectedKinematics);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets current estimation containing user ECEF position, user ECEF velocity,
     * clock offset and clock drift.
     *
     * @return current estimation containing user ECEF position, user ECEF velocity,
     * clock offset and clock drift.
     */
    public GNSSEstimation getEstimation() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3431
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3657
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3713
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4054
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4099
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4328
                return initial;
            }

            @Override
            public void evaluate(
                    final int i, final double[] point, final double[] result,
                    final double[] params, final Matrix jacobian) {
                // We know that:
                //  fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                //  fmeasy = by + ftruey + sy * ftruey + myz * ftruez
                //  fmeasz = bz + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
                // sz, mxy, mxz, myz

                // d(fmeasx)/d(bx) = 1.0
                // d(fmeasx)/d(by) = 0.0
                // d(fmeasx)/d(bz) = 0.0
                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myz) = 0.0

                // d(fmeasy)/d(bx) = 0.0
                // d(fmeasy)/d(by) = 1.0
                // d(fmeasy)/d(bz) = 0.0
                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myz) = ftruez

                // d(fmeasz)/d(bx) = 0.0
                // d(fmeasz)/d(by) = 0.0
                // d(fmeasz)/d(bz) = 1.0
                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myz) = 0.0

                final double bx = params[0];
                final double by = params[1];
                final double bz = params[2];

                final double sx = params[3];
                final double sy = params[4];
                final double sz = params[5];

                final double mxy = params[6];
                final double mxz = params[7];
                final double myz = params[8];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 240
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 712
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 697
    public void setStopThreshold(double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
                new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3283
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3557
            final double[] bias, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        bias, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        bias, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        bias, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        bias, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        bias, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param bias           known accelerometer bias.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double[] bias, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 2730
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 2807
            return Math.sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ);

        } catch (final WrongSizeException e) {
            return Double.MAX_VALUE;
        }
    }

    /**
     * Computes a preliminary solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminary solution will be stored.
     */
    protected void computePreliminarySolutions(final int[] samplesIndices,
                                               final List<PreliminaryResult> solutions) {

        final List<StandardDeviationFrameBodyKinematics> measurements = new ArrayList<>();

        for (final int samplesIndex : samplesIndices) {
            measurements.add(mMeasurements.get(samplesIndex));
        }

        try {
            final PreliminaryResult result = new PreliminaryResult();
            result.mEstimatedBiases = getInitialBias();
            result.mEstimatedMa = getInitialMa();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3448
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3221
                        for (int i = 0, j = BodyKinematics.COMPONENTS + num; i < num; i++, j++) {
                            initial[j] = initialG.getElementAtIndex(i);
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {
                        mI = i;

                        // point contains fixed gravity versor values for current
                        // sequence
                        mPoint = point;

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateGeneralWithGDependentCrossBiases(i, params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3991
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5293
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5614
        if (mEstimatedMg == null) {
            mEstimatedMg = m;
        } else {
            mEstimatedMg.copyFrom(m);
        }

        for (int i = 0; i < BodyKinematics.COMPONENTS; i++) {
            mEstimatedMg.setElementAt(i, i,
                    mEstimatedMg.getElementAt(i, i) - 1.0);
        }

        if (mEstimatedGg == null) {
            mEstimatedGg = new Matrix(
                    BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            mEstimatedGg.initialize(0.0);
        }

        mEstimatedCovariance = mFitter.getCovar();
        mEstimatedChiSq = mFitter.getChisq();
    }

    /**
     * Computes gravity versor error at the end of a sequence using provided
     * parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param i      row position.
     * @param params array containing parameters for the general purpose case
     *               when G-dependent cross biases are taken into account. Must
     *               have length 18.
     * @return error between estimated and measured gravity versor.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneralWithGDependentCrossBiases(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4709
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4966
                        for (int i = 0, j = num; i < num; i++, j++) {
                            initial[j] = initialG.getElementAtIndex(i);
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {

                        mMeasAngularRateX = point[0];
                        mMeasAngularRateY = point[1];
                        mMeasAngularRateZ = point[2];

                        mFmeasX = point[3];
                        mFmeasY = point[4];
                        mFmeasZ = point[5];

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateGeneralWitGDependentCrossBiases(params);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 199
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 180
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 196
            final RobustKnownFrameGyroscopeCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<PreliminaryResult> innerEstimator =
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4158
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5430
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4308
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5266
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3358
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3632
            final double[] bias, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        bias, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        bias, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        bias, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        bias, commonAxisUsed, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        bias, commonAxisUsed, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param bias           known gyroscope bias.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double[] bias, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3952
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5224
            final double[] accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4102
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5060
            final Matrix accelerometerBias,
            final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case LMedS:
                return new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case MSAC:
                return new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                        sequences, commonAxisUsed, estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg, accelerometerBias,
                        accelerometerMa, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5211
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6533
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8112
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9520
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias,
                        initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5380
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6702
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7931
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 9339
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg,
                        initialGg);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5455
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6837
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8514
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10004
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5637
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7019
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8322
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9810
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, commonAxisUsed,
                        estimateGDependentCrossBiases,
                        initialBias, initialMg, initialGg);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8205
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10000
                                    final double fz,
                                    final double angularRateX,
                                    final double angularRateY,
                                    final double angularRateZ,
                                    final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static void navigateECEF(final double timeInterval,
                                    final double oldX,
                                    final double oldY,
                                    final double oldZ,
                                    final CoordinateTransformation oldC,
                                    final double oldVx,
                                    final double oldVy,
                                    final double oldVz,
                                    final BodyKinematics kinematics,
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator2D.java 432
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator2D.java 444
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator2D.java 975
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator2D.java 469
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 1000
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 491
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 1020
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 516
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator2D.java 280
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator2D.java 527
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point2D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point2D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return MSACRobustRangingAndRssiRadioSourceEstimator2D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator3D.java 432
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator3D.java 446
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator3D.java 973
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator3D.java 468
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator3D.java 987
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 1005
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 490
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 1017
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator3D.java 516
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator3D.java 280
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 525
                            }

                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point3D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point3D> currentEstimation,
                                    final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return MSACRobustRangingAndRssiRadioSourceEstimator3D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator2D.java 930
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator2D.java 1736
            final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings,
            final Point2D initialPosition,
            final Double initialTransmittedPowerdBm,
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustRangingAndRssiRadioSourceEstimator2D<>(
                        readings, initialPosition, initialTransmittedPowerdBm, listener);
            case LMedS:
                return new LMedSRobustRangingAndRssiRadioSourceEstimator2D<>(
                        readings, initialPosition, initialTransmittedPowerdBm, listener);
            case MSAC:
                return new MSACRobustRangingAndRssiRadioSourceEstimator2D<>(
                        readings, initialPosition, initialTransmittedPowerdBm, listener);
            case PROSAC:
                return new PROSACRobustRangingAndRssiRadioSourceEstimator2D<>(
File Line
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator3D.java 938
com/irurueta/navigation/indoor/radiosource/RobustRangingAndRssiRadioSourceEstimator3D.java 1744
            final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
            final Point3D initialPosition,
            final Double initialTransmittedPowerdBm,
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustRangingAndRssiRadioSourceEstimator3D<>(
                        readings, initialPosition, initialTransmittedPowerdBm, listener);
            case LMedS:
                return new LMedSRobustRangingAndRssiRadioSourceEstimator3D<>(
                        readings, initialPosition, initialTransmittedPowerdBm, listener);
            case MSAC:
                return new MSACRobustRangingAndRssiRadioSourceEstimator3D<>(
                        readings, initialPosition, initialTransmittedPowerdBm, listener);
            case PROSAC:
                return new PROSACRobustRangingAndRssiRadioSourceEstimator3D<>(
File Line
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator2D.java 942
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator2D.java 1747
            final List<? extends RssiReadingLocated<S, Point2D>> readings,
            final Point2D initialPosition,
            final Double initialTransmittedPowerdBm,
            final RobustRssiRadioSourceEstimatorListener<S, Point2D> listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustRssiRadioSourceEstimator2D<>(
                        readings, initialPosition, initialTransmittedPowerdBm, listener);
            case LMedS:
                return new LMedSRobustRssiRadioSourceEstimator2D<>(
                        readings, initialPosition, initialTransmittedPowerdBm, listener);
            case MSAC:
                return new MSACRobustRssiRadioSourceEstimator2D<>(
                        readings, initialPosition, initialTransmittedPowerdBm, listener);
            case PROSAC:
                return new PROSACRobustRssiRadioSourceEstimator2D<>(
File Line
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator3D.java 942
com/irurueta/navigation/indoor/radiosource/RobustRssiRadioSourceEstimator3D.java 1749
            final List<? extends RssiReadingLocated<S, Point3D>> readings,
            final Point3D initialPosition,
            final Double initialTransmittedPowerdBm,
            final RobustRssiRadioSourceEstimatorListener<S, Point3D> listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustRssiRadioSourceEstimator3D<>(
                        readings, initialPosition, initialTransmittedPowerdBm, listener);
            case LMedS:
                return new LMedSRobustRssiRadioSourceEstimator3D<>(
                        readings, initialPosition, initialTransmittedPowerdBm, listener);
            case MSAC:
                return new MSACRobustRssiRadioSourceEstimator3D<>(
                        readings, initialPosition, initialTransmittedPowerdBm, listener);
            case PROSAC:
                return new PROSACRobustRssiRadioSourceEstimator3D<>(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6214
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1227
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1616
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2363
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3597
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1255
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2369
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3555
                initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    public double[] getBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = mBiasX;
        result[1] = mBiasY;
        result[2] = mBiasZ;
    }

    /**
     * Sets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known bias to be set
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setBias(final double[] bias) throws LockedException {
        if (mRunning) {
            throw new LockedException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6574
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7013
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2069
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2085
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException
                | com.irurueta.numerical.NotReadyException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 757
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1723
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 832
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 222
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 983
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 505
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2210
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 295
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1056
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<Matrix> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final Matrix currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 841
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 218
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 849
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1830
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 501
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1851
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 915
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 295
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 927
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 766
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1888
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 226
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1918
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1730
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3996
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 507
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 4059
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 839
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1963
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 302
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1993
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 207
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 984
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 504
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2208
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 295
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1058
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mMeasurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation, final int i) {
                        return computeError(mMeasurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2670
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2970
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2792
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4155
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3083
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4496
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4655
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4979
            final double[] bias,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        measurements, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(
                        qualityScores, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param bias           known accelerometer bias.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided quality scores length is smaller than
     *                                  4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2800
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3619
            final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2957
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3461
            final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3199
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4608
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias,
            final Matrix initialMa,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3861
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5359
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias,
            final Matrix initialMa,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, bias, initialMa,
                        listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2978
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3809
            final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3138
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3649
            final Matrix initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, initialBias, listener);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3390
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4831
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias,
            final Matrix initialMa,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4061
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5603
            final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias,
            final Matrix initialMa,
            final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(
                        position, measurements, commonAxisUsed, initialBias,
                        initialMa, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4524
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4709
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4765
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4966
                        for (int i = 0, j = k; i < num; i++, j++) {
                            initial[j] = initialG.getElementAtIndex(i);
                        }

                        return initial;
                    }

                    @Override
                    public double evaluate(
                            final int i, final double[] point,
                            final double[] params, final double[] derivatives)
                            throws EvaluationException {

                        mMeasAngularRateX = point[0];
                        mMeasAngularRateY = point[1];
                        mMeasAngularRateZ = point[2];

                        mFmeasX = point[3];
                        mFmeasY = point[4];
                        mFmeasZ = point[5];

                        gradientEstimator.gradient(params, derivatives);

                        return evaluateCommonAxisWitGDependentCrossBiases(params);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 692
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 677
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1498
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1468
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 767
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 752
                    @Override
                    public double getThreshold() {
                        return mThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return mSequences.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return mPreliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices,
                            final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(
                            final PreliminaryResult currentEstimation,
                            final int i) {
                        return computeError(mSequences.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return MSACRobustEasyGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2741
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3044
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        biasX, biasY, biasZ, commonAxisUsed);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2863
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4231
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3158
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4576
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final AngularSpeed biasX, final AngularSpeed biasY,
            final AngularSpeed biasZ, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4735
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5057
            final double[] bias,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements,
                        bias, listener);
            case LMedS:
                return new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(measurements,
                        bias, listener);
            case MSAC:
                return new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements,
                        bias, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, measurements, bias, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                        qualityScores, measurements, bias, listener);
        }
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param bias           known gyroscope bias.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided quality scores length is smaller
     *                                  than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3006
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3922
            final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3183
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3745
            final Matrix hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3464
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5279
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron,
            final Matrix initialMm,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4203
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 6126
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron,
            final Matrix initialMm,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, hardIron,
                        initialMm, listener);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3115
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4031
            final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3292
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3854
            final Matrix initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron, listener);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3573
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5388
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final Matrix initialMm,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4312
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6235
            final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final Matrix initialMm,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, commonAxisUsed, initialHardIron,
                        initialMm, listener);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 100
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 161
    public void navigate(final double timeInterval,
                         final double oldX,
                         final double oldY,
                         final double oldZ,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final double fx,
                         final double fy,
                         final double fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final ECEFFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 1122
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 1183
    public void navigate(final double timeInterval,
                         final Distance oldX,
                         final Distance oldY,
                         final Distance oldZ,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final double fx,
                         final double fy,
                         final double fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final ECEFFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 1498
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 1560
    public void navigate(final double timeInterval,
                         final double oldX,
                         final double oldY,
                         final double oldZ,
                         final CoordinateTransformation oldC,
                         final Speed oldSpeedX,
                         final Speed oldSpeedY,
                         final Speed oldSpeedZ,
                         final double fx,
                         final double fy,
                         final double fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final ECEFFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY,
                oldSpeedZ, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 1875
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 1933
    public void navigate(final double timeInterval,
                         final double oldX,
                         final double oldY,
                         final double oldZ,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final Acceleration fx,
                         final Acceleration fy,
                         final Acceleration fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final ECEFFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2283
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2341
    public void navigate(final double timeInterval,
                         final double oldX,
                         final double oldY,
                         final double oldZ,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final double fx,
                         final double fy,
                         final double fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final ECEFFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2394
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10215
    public void navigate(final double timeInterval,
                         final ECEFPosition oldPosition,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final double fx,
                         final double fy,
                         final double fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2446
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10163
    public void navigate(final Time timeInterval,
                         final ECEFPosition oldPosition,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final double fx,
                         final double fy,
                         final double fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final double timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2696
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2758
    public void navigate(final double timeInterval,
                         final Distance oldX,
                         final Distance oldY,
                         final Distance oldZ,
                         final CoordinateTransformation oldC,
                         final Speed oldSpeedX,
                         final Speed oldSpeedY,
                         final Speed oldSpeedZ,
                         final double fx,
                         final double fy,
                         final double fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final ECEFFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY,
                oldSpeedZ, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2814
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2870
    public void navigate(final double timeInterval,
                         final Distance oldX,
                         final Distance oldY,
                         final Distance oldZ,
                         final CoordinateTransformation oldC,
                         final Speed oldSpeedX,
                         final Speed oldSpeedY,
                         final Speed oldSpeedZ,
                         final Acceleration fx,
                         final Acceleration fy,
                         final Acceleration fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final ECEFFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY,
                oldSpeedZ, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 3200
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 3255
    public void navigate(final double timeInterval,
                         final double oldX,
                         final double oldY,
                         final double oldZ,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final Acceleration fx,
                         final Acceleration fy,
                         final Acceleration fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final ECEFFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 91
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 153
    public void navigate(final double timeInterval,
                         final double oldX,
                         final double oldY,
                         final double oldZ,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final double fx,
                         final double fy,
                         final double fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 478
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 539
    public void navigate(final double timeInterval,
                         final Distance oldX,
                         final Distance oldY,
                         final Distance oldZ,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final double fx,
                         final double fy,
                         final double fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 682
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 744
    public void navigate(final double timeInterval,
                         final double oldX,
                         final double oldY,
                         final double oldZ,
                         final CoordinateTransformation oldC,
                         final Speed oldSpeedX,
                         final Speed oldSpeedY,
                         final Speed oldSpeedZ,
                         final double fx,
                         final double fy,
                         final double fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC,
                oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 885
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 943
    public void navigate(final double timeInterval,
                         final double oldX,
                         final double oldY,
                         final double oldZ,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final Acceleration fx,
                         final Acceleration fy,
                         final Acceleration fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 1001
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 1059
    public void navigate(final double timeInterval,
                         final double oldX,
                         final double oldY,
                         final double oldZ,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final double fx,
                         final double fy,
                         final double fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 1120
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 1182
    public void navigate(final double timeInterval,
                         final Distance oldX,
                         final Distance oldY,
                         final Distance oldZ,
                         final CoordinateTransformation oldC,
                         final Speed oldSpeedX,
                         final Speed oldSpeedY,
                         final Speed oldSpeedZ,
                         final double fx,
                         final double fy,
                         final double fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC,
                oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 1238
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 1294
    public void navigate(final double timeInterval,
                         final Distance oldX,
                         final Distance oldY,
                         final Distance oldZ,
                         final CoordinateTransformation oldC,
                         final Speed oldSpeedX,
                         final Speed oldSpeedY,
                         final Speed oldSpeedZ,
                         final Acceleration fx,
                         final Acceleration fy,
                         final Acceleration fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC,
                oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 1350
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 1405
    public void navigate(final double timeInterval,
                         final double oldX,
                         final double oldY,
                         final double oldZ,
                         final CoordinateTransformation oldC,
                         final double oldVx,
                         final double oldVy,
                         final double oldVz,
                         final Acceleration fx,
                         final Acceleration fy,
                         final Acceleration fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 101
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 163
    public void navigate(final double timeInterval,
                         final double oldLatitude,
                         final double oldLongitude,
                         final double oldHeight,
                         final CoordinateTransformation oldC,
                         final double oldVn,
                         final double oldVe,
                         final double oldVd,
                         final double fx,
                         final double fy,
                         final double fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 841
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 903
    public void navigate(final double timeInterval,
                         final Angle oldLatitude,
                         final Angle oldLongitude,
                         final Distance oldHeight,
                         final CoordinateTransformation oldC,
                         final double oldVn,
                         final double oldVe,
                         final double oldVd,
                         final double fx,
                         final double fy,
                         final double fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 1225
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 1287
    public void navigate(final double timeInterval,
                         final double oldLatitude,
                         final double oldLongitude,
                         final double oldHeight,
                         final CoordinateTransformation oldC,
                         final Speed oldSpeedN,
                         final Speed oldSpeedE,
                         final Speed oldSpeedD,
                         final double fx,
                         final double fy,
                         final double fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 1616
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 1675
    public void navigate(final double timeInterval,
                         final double oldLatitude,
                         final double oldLongitude,
                         final double oldHeight,
                         final CoordinateTransformation oldC,
                         final double oldVn,
                         final double oldVe,
                         final double oldVd,
                         final Acceleration fx,
                         final Acceleration fy,
                         final Acceleration fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2048
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2107
    public void navigate(final double timeInterval,
                         final double oldLatitude,
                         final double oldLongitude,
                         final double oldHeight,
                         final CoordinateTransformation oldC,
                         final double oldVn,
                         final double oldVe,
                         final double oldVd,
                         final double fx,
                         final double fy,
                         final double fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2483
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2545
    public void navigate(final double timeInterval,
                         final Angle oldLatitude,
                         final Angle oldLongitude,
                         final Distance oldHeight,
                         final CoordinateTransformation oldC,
                         final Speed oldSpeedN,
                         final Speed oldSpeedE,
                         final Speed oldSpeedD,
                         final double fx,
                         final double fy,
                         final double fz,
                         final double angularRateX,
                         final double angularRateY,
                         final double angularRateZ,
                         final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2601
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2657
    public void navigate(final double timeInterval,
                         final Angle oldLatitude,
                         final Angle oldLongitude,
                         final Distance oldHeight,
                         final CoordinateTransformation oldC,
                         final Speed oldSpeedN,
                         final Speed oldSpeedE,
                         final Speed oldSpeedD,
                         final Acceleration fx,
                         final Acceleration fy,
                         final Acceleration fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2713
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2769
    public void navigate(final double timeInterval,
                         final double oldLatitude,
                         final double oldLongitude,
                         final double oldHeight,
                         final CoordinateTransformation oldC,
                         final Speed oldSpeedN,
                         final Speed oldSpeedE,
                         final Speed oldSpeedD,
                         final Acceleration fx,
                         final Acceleration fy,
                         final Acceleration fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 3121
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 3177
    public void navigate(final double timeInterval,
                         final Angle oldLatitude,
                         final Angle oldLongitude,
                         final Distance oldHeight,
                         final CoordinateTransformation oldC,
                         final double oldVn,
                         final double oldVe,
                         final double oldVd,
                         final Acceleration fx,
                         final Acceleration fy,
                         final Acceleration fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 3337
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 3393
    public void navigate(final double timeInterval,
                         final double oldLatitude,
                         final double oldLongitude,
                         final double oldHeight,
                         final CoordinateTransformation oldC,
                         final double oldVn,
                         final double oldVe,
                         final double oldVd,
                         final Acceleration fx,
                         final Acceleration fy,
                         final Acceleration fz,
                         final AngularSpeed angularRateX,
                         final AngularSpeed angularRateY,
                         final AngularSpeed angularRateZ,
                         final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(final Time timeInterval,
File Line
com/irurueta/navigation/lateration/HomogeneousLinearLeastSquaresLateration2DSolver.java 97
com/irurueta/navigation/lateration/InhomogeneousLinearLeastSquaresLateration2DSolver.java 97
com/irurueta/navigation/lateration/NonLinearLeastSquaresLateration2DSolver.java 305
        internalSetCircles(circles);
    }

    /**
     * Gets circles defined by provided positions and distances.
     *
     * @return circles defined by provided positions and distances.
     */
    public Circle[] getCircles() {
        if (mPositions == null) {
            return null;
        }

        final Circle[] result = new Circle[mPositions.length];

        for (int i = 0; i < mPositions.length; i++) {
            result[i] = new Circle(mPositions[i], mDistances[i]);
        }
        return result;
    }

    /**
     * Sets circles defining positions and euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 2.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setCircles(final Circle[] circles) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetCircles(circles);
    }
File Line
com/irurueta/navigation/lateration/HomogeneousLinearLeastSquaresLateration3DSolver.java 97
com/irurueta/navigation/lateration/InhomogeneousLinearLeastSquaresLateration3DSolver.java 97
com/irurueta/navigation/lateration/NonLinearLeastSquaresLateration3DSolver.java 305
        internalSetSpheres(spheres);
    }

    /**
     * Gets spheres defined by provided positions and distances.
     *
     * @return spheres defined by provided positions and distances.
     */
    public Sphere[] getSpheres() {
        if (mPositions == null) {
            return null;
        }

        final Sphere[] result = new Sphere[mPositions.length];

        for (int i = 0; i < mPositions.length; i++) {
            result[i] = new Sphere(mPositions[i], mDistances[i]);
        }
        return result;
    }

    /**
     * Sets spheres defining positions and euclidean distances.
     *
     * @param spheres spheres defining positions and distances.
     * @throws IllegalArgumentException if spheres is null or length of array of spheres
     *                                  is less than 2.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setSpheres(final Sphere[] spheres) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSpheres(spheres);
    }
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingRadioSourceEstimator2D.java 176
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator3D.java 412
        super(readings, initialPosition, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Robustly estimates position for a radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException,
            RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingRadioSourceEstimator2D.java 255
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingRadioSourceEstimator2D.java 211
                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] sampleIndices,
                                    final List<Solution<Point2D>> solutions) {
                                solvePreliminarSolutions(sampleIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point2D> currentEstimation, final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return LMedSRobustRangingRadioSourceEstimator2D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingRadioSourceEstimator3D.java 177
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator2D.java 412
        super(readings, initialPosition, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algrithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return mStopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        mStopThreshold = stopThreshold;
    }

    /**
     * Robustly estimates position for a radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException,
            RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final LMedSRobustEstimator<Solution<Point3D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRangingRadioSourceEstimator3D.java 256
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingRadioSourceEstimator3D.java 209
                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] sampleIndices,
                                    final List<Solution<Point3D>> solutions) {
                                solvePreliminarSolutions(sampleIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point3D> currentEstimation, final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return LMedSRobustRangingRadioSourceEstimator3D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator2D.java 491
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator2D.java 434
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator2D.java 446
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator2D.java 977
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator2D.java 471
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator2D.java 1002
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator2D.java 493
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator2D.java 1022
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 518
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator2D.java 282
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator2D.java 529
                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point2D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point2D> currentEstimation, final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return LMedSRobustRssiRadioSourceEstimator2D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/LMedSRobustRssiRadioSourceEstimator3D.java 491
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator3D.java 434
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator3D.java 448
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingAndRssiRadioSourceEstimator3D.java 975
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRangingRadioSourceEstimator3D.java 470
com/irurueta/navigation/indoor/radiosource/PROMedSRobustRssiRadioSourceEstimator3D.java 989
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingAndRssiRadioSourceEstimator3D.java 1007
com/irurueta/navigation/indoor/radiosource/PROSACRobustRangingRadioSourceEstimator3D.java 492
com/irurueta/navigation/indoor/radiosource/PROSACRobustRssiRadioSourceEstimator3D.java 1019
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator3D.java 518
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingRadioSourceEstimator3D.java 282
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 527
                            @Override
                            public int getTotalSamples() {
                                return mReadings.size();
                            }

                            @Override
                            public int getSubsetSize() {
                                return Math.max(mPreliminarySubsetSize, getMinReadings());
                            }

                            @Override
                            public void estimatePreliminarSolutions(
                                    final int[] samplesIndices,
                                    final List<Solution<Point3D>> solutions) {
                                solvePreliminarSolutions(samplesIndices, solutions);
                            }

                            @Override
                            public double computeResidual(
                                    final Solution<Point3D> currentEstimation, final int i) {
                                return residual(currentEstimation, i);
                            }

                            @Override
                            public boolean isReady() {
                                return LMedSRobustRssiRadioSourceEstimator3D.this.isReady();
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator2D.java 371
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator2D.java 399
    public MSACRobustRangingAndRssiRadioSourceEstimator2D(
            final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings,
            final Point2D initialPosition, final Double initialTransmittedPowerdBm,
            final double initialPathLossExponent,
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if robust estimator is locked because an
     *                                  estimation is already in progress.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator2D.java 377
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingRadioSourceEstimator3D.java 152
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator2D.java 389
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if robust estimator is locked because an
     *                                  estimation is already in progress.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<Solution<Point2D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator3D.java 371
com/irurueta/navigation/indoor/radiosource/RANSACRobustRangingAndRssiRadioSourceEstimator3D.java 399
    public MSACRobustRangingAndRssiRadioSourceEstimator3D(
            final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
            final Point3D initialPosition, final Double initialTransmittedPowerdBm,
            final double initialPathLossExponent,
            final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if robust estimator is locked because an
     *                                  estimation is already in progress.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingAndRssiRadioSourceEstimator3D.java 377
com/irurueta/navigation/indoor/radiosource/MSACRobustRangingRadioSourceEstimator2D.java 154
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator3D.java 391
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if robust estimator is locked because an
     *                                  estimation is already in progress.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }

    /**
     * Robustly estimates position, transmitted power and pathloss exponent for a
     * radio source.
     *
     * @throws LockedException          if instance is busy during estimation.
     * @throws NotReadyException        if estimator is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final MSACRobustEstimator<Solution<Point3D>> innerEstimator =
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator2D.java 383
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator2D.java 410
    public MSACRobustRssiRadioSourceEstimator2D(
            final List<? extends RssiReadingLocated<S, Point2D>> readings,
            final Point2D initialPosition, final Double initialTransmittedPowerdBm,
            final double initialPathLossExponent,
            final RobustRssiRadioSourceEstimatorListener<S, Point2D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if robust estimator is locked because an
     *                                  estimation is already in progress.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }
File Line
com/irurueta/navigation/indoor/radiosource/MSACRobustRssiRadioSourceEstimator3D.java 384
com/irurueta/navigation/indoor/radiosource/RANSACRobustRssiRadioSourceEstimator3D.java 409
    public MSACRobustRssiRadioSourceEstimator3D(
            final List<? extends RssiReadingLocated<S, Point3D>> readings,
            final Point3D initialPosition,
            final Double initialTransmittedPowerdBm,
            final double initialPathLossExponent,
            final RobustRssiRadioSourceEstimatorListener<S, Point3D> listener) {
        super(readings, initialPosition, initialTransmittedPowerdBm,
                initialPathLossExponent, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return mThreshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if robust estimator is locked because an
     *                                  estimation is already in progress.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        mThreshold = threshold;
    }
File Line
com/irurueta/navigation/inertial/calibration/AccelerationFixer.java 920
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1100
            final double mzx, final double mzy,
            final double[] result) throws AlgebraException {

        mCrossCouplingErrors.setElementAt(0, 0, sx);
        mCrossCouplingErrors.setElementAt(1, 1, sy);
        mCrossCouplingErrors.setElementAt(2, 2, sz);
        mCrossCouplingErrors.setElementAt(0, 1, mxy);
        mCrossCouplingErrors.setElementAt(0, 2, mxz);
        mCrossCouplingErrors.setElementAt(1, 0, myx);
        mCrossCouplingErrors.setElementAt(1, 2, myz);
        mCrossCouplingErrors.setElementAt(2, 0, mzx);
        mCrossCouplingErrors.setElementAt(2, 1, mzy);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2698
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2965
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3078
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3224
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 2079
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 2095
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException |
                com.irurueta.numerical.NotReadyException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6584
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7023
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3711
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3670
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (mRunning) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            mRunning = true;

            if (mListener != null) {
                mListener.onCalibrateStart(this);
            }

            if (mCommonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException
                | com.irurueta.numerical.NotReadyException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 6808
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 7415
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5212
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5522
        mFitter.setInputData(x, y, specificForceStandardDeviations);
    }

    /**
     * Converts provided NED position expressed in terms of latitude, longitude and height respect
     * mean Earth surface, to position expressed in ECEF coordinates.
     *
     * @param position NED position to be converted.
     * @return converted position expressed in ECEF coordinates.
     */
    private static ECEFPosition convertPosition(final NEDPosition position) {
        final ECEFVelocity velocity = new ECEFVelocity();
        final ECEFPosition result = new ECEFPosition();
        NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                position.getLatitude(), position.getLongitude(), position.getHeight(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(),
                acceleration.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws FittingException                         if Levenberg-Marquardt fails for numerical reasons.
     * @throws AlgebraException                         if there are numerical instabilities that prevent
     *                                                  matrix inversion.
     * @throws com.irurueta.numerical.NotReadyException never happens.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3494
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3732
                result[2] = bz + ftruez + sz * ftruez;

                jacobian.setElementAt(0, 0, 1.0);
                jacobian.setElementAt(0, 1, 0.0);
                jacobian.setElementAt(0, 2, 0.0);
                jacobian.setElementAt(0, 3, ftruex);
                jacobian.setElementAt(0, 4, 0.0);
                jacobian.setElementAt(0, 5, 0.0);
                jacobian.setElementAt(0, 6, ftruey);
                jacobian.setElementAt(0, 7, ftruez);
                jacobian.setElementAt(0, 8, 0.0);

                jacobian.setElementAt(1, 0, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 912
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 291
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 922
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 768
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 839
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 753
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1959
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 299
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1991
            mInliersData = null;
            innerEstimator.setConfidence(mConfidence);
            innerEstimator.setMaxIterations(mMaxIterations);
            innerEstimator.setProgressDelta(mProgressDelta);
            final PreliminaryResult preliminaryResult = innerEstimator.estimate();
            mInliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (mListener != null) {
                mListener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 78
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 77
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 84
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 83
    public static final boolean DEFAULT_USE_LINEAR_CALIBRATOR = true;

    /**
     * Indicates that by default preliminary solutions are refined.
     */
    public static final boolean DEFAULT_REFINE_PRELIMINARY_SOLUTIONS = false;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD =
            RobustEstimatorMethod.LMedS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen subsamples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Contains a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     */
    protected List<StandardDeviationFrameBodyKinematics> mMeasurements;

    /**
     * Listener to be notified of events such as when calibration starts, ends or its
     * progress significantly changes.
     */
    protected RobustKnownBiasAndFrameAccelerometerCalibratorListener mListener;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2760
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3421
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case LMedS:
                return new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case MSAC:
                return new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case PROSAC:
                return new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                        position, measurements, bias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias. This must have length 3 and is
     *                     expressed in meters per squared second (m/s^2).
     * @param listener     listener to handle events raised by this calibrator.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2937
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3608
            final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias, final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias);
            case LMedS:
                return new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias);
            case MSAC:
                return new MSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias);
            case PROSAC:
                return new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                        measurements, initialBias);
        }
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial accelerometer bias to be used to find a solution.
     *                     This must have length 3 and is expressed in meters per
     *                     squared second (m/s^2).
     * @param listener     listener to handle events raised by this calibrator.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3300
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3652
                        return evaluateCommonAxisWithGDependentCrossBiases(i, params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double bx = result[0];
        final double by = result[1];
        final double bz = result[2];

        final double m11 = result[3];

        final double m12 = result[4];
        final double m22 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final double g11 = result[9];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3241
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3530
                        return evaluateGeneralWithGDependentCrossBiases(i, params);
                    }
                });

        setInputData();

        mFitter.fit();

        final double[] result = mFitter.getA();

        final double m11 = result[0];
        final double m21 = result[1];
        final double m31 = result[2];

        final double m12 = result[3];
        final double m22 = result[4];
        final double m32 = result[5];

        final double m13 = result[6];
        final double m23 = result[7];
        final double m33 = result[8];

        final double g11 = result[9];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4677
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5999
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7501
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8911
            final Matrix bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval, measurements,
                        bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4812
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6134
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7356
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8764
            final double[] bias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case LMedS:
                return new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case MSAC:
                return new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, bias, initialMg, initialGg, listener);
            case PROSAC:
                return new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4897
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6284
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7878
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9365
            final Matrix initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg, initialGg,
                        listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5044
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6426
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7719
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9209
            final double[] initialBias,
            final Matrix initialMg,
            final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case LMedS:
                return new LMedSRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case MSAC:
                return new MSACRobustTurntableGyroscopeCalibrator(
                        position, turntableRotationRate, timeInterval,
                        measurements, initialBias, initialMg,
                        initialGg, listener);
            case PROSAC:
                return new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4162
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4404
                result[2] = bz + btruez + sz * btruez;

                jacobian.setElementAt(0, 0, 1.0);
                jacobian.setElementAt(0, 1, 0.0);
                jacobian.setElementAt(0, 2, 0.0);
                jacobian.setElementAt(0, 3, btruex);
                jacobian.setElementAt(0, 4, 0.0);
                jacobian.setElementAt(0, 5, 0.0);
                jacobian.setElementAt(0, 6, btruey);
                jacobian.setElementAt(0, 7, btruez);
                jacobian.setElementAt(0, 8, 0.0);

                jacobian.setElementAt(1, 0, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 84
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 84
    public static final boolean DEFAULT_USE_LINEAR_CALIBRATOR = true;

    /**
     * Indicates that by default preliminary solutions are refined.
     */
    public static final boolean DEFAULT_REFINE_PRELIMINARY_SOLUTIONS = false;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD =
            RobustEstimatorMethod.LMedS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen subsamples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Contains a list of body magnetic flux density measurements taken
     * at different frames (positions and orientations) and containing the
     * standard deviation of magnetometer measurements.
     * If a single device magnetometer needs to be calibrated, typically all
     * measurements are taken at the same position, with zero velocity and
     * multiple orientations.
     * However, if we just want to calibrate a given magnetometer model (e.g.
     * obtain an average and less precise calibration for the magnetometer of
     * a given phone model), we could take measurements collected throughout
     * the planet at multiple positions while the phone remains static (e.g.
     * while charging), hence each measurement position will change, velocity
     * will remain zero and orientation will be typically constant at
     * horizontal orientation while the phone remains on a
     * flat surface.
     */
    protected List<StandardDeviationFrameBodyMagneticFluxDensity> mMeasurements;

    /**
     * Listener to be notified of events such as when calibration starts, ends or its
     * progress significantly changes.
     */
    protected RobustKnownFrameMagnetometerCalibratorListener mListener;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2962
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3701
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] hardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case LMedS:
                return new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case MSAC:
                return new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case PROSAC:
                return new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                        position, measurements, hardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param listener     listener to handle events raised by this calibrator.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3071
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3810
            final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        switch (method) {
            case RANSAC:
                return new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case LMedS:
                return new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case MSAC:
                return new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case PROSAC:
                return new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
            case PROMedS:
            default:
                return new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                        position, measurements, initialHardIron);
        }
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11826
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12872
                                                     final double oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final double oldVx,
                                                     final double oldVy,
                                                     final double oldVz,
                                                     final double fx,
                                                     final double fy,
                                                     final double fz,
                                                     final double angularRateX,
                                                     final double angularRateY,
                                                     final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
                                                     final double oldX,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11833
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 13646
                                                     final double fz,
                                                     final double angularRateX,
                                                     final double angularRateY,
                                                     final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
                                                     final double oldX,
                                                     final double oldY,
                                                     final double oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final double oldVx,
                                                     final double oldVy,
                                                     final double oldVz,
                                                     final double fx,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11836
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 14068
                                                     final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
                                                     final double oldX,
                                                     final double oldY,
                                                     final double oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final double oldVx,
                                                     final double oldVy,
                                                     final double oldVz,
                                                     final double fx,
                                                     final double fy,
                                                     final double fz,
                                                     final double angularRateX,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11888
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12934
                                                     final double oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final double oldVx,
                                                     final double oldVy,
                                                     final double oldVz,
                                                     final double fx,
                                                     final double fy,
                                                     final double fz,
                                                     final double angularRateX,
                                                     final double angularRateY,
                                                     final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final double timeInterval,
                                                     final ECEFPosition oldPosition,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 13256
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 14477
                                                     final double oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final Speed oldSpeedX,
                                                     final Speed oldSpeedY,
                                                     final Speed oldSpeedZ,
                                                     final double fx,
                                                     final double fy,
                                                     final double fz,
                                                     final double angularRateX,
                                                     final double angularRateY,
                                                     final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY,
                oldSpeedZ, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
                                                     final double oldX,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 13318
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 14540
                                                     final double oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final Speed oldSpeedX,
                                                     final Speed oldSpeedY,
                                                     final Speed oldSpeedZ,
                                                     final double fx,
                                                     final double fy,
                                                     final double fz,
                                                     final double angularRateX,
                                                     final double angularRateY,
                                                     final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY,
                oldSpeedZ, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final double timeInterval,
                                                     final ECEFPosition oldPosition,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 13649
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 15001
                                                     final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy,
                fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
                                                     final double oldX,
                                                     final double oldY,
                                                     final double oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final double oldVx,
                                                     final double oldVy,
                                                     final double oldVz,
                                                     final Acceleration fx,
                                                     final Acceleration fy,
                                                     final Acceleration fz,
                                                     final double angularRateX,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 14065
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 14998
                                                     final double fz,
                                                     final AngularSpeed angularRateX,
                                                     final AngularSpeed angularRateY,
                                                     final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECEFFrame result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy,
                fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(final Time timeInterval,
                                                     final double oldX,
                                                     final double oldY,
                                                     final double oldZ,
                                                     final CoordinateTransformation oldC,
                                                     final double oldVx,
                                                     final double oldVy,
                                                     final double oldVz,
                                                     final double fx,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5670
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6065
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVx,
                                                   final double oldVy,
                                                   final double oldVz,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
                                                   final double oldX,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5677
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6485
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
                                                   final double oldX,
                                                   final double oldY,
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVx,
                                                   final double oldVy,
                                                   final double oldVz,
                                                   final double fx,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5680
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6606
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
                                                   final double oldX,
                                                   final double oldY,
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVx,
                                                   final double oldVy,
                                                   final double oldVz,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5733
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6127
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVx,
                                                   final double oldVy,
                                                   final double oldVz,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final double timeInterval,
                                                   final double oldX,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5740
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6544
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final double timeInterval,
                                                   final double oldX,
                                                   final double oldY,
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVx,
                                                   final double oldVy,
                                                   final double oldVz,
                                                   final BodyKinematics kinematics)
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6273
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6717
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedX,
                                                   final Speed oldSpeedY,
                                                   final Speed oldSpeedZ,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
                                                   final double oldX,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6335
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6779
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedX,
                                                   final Speed oldSpeedY,
                                                   final Speed oldSpeedZ,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final double timeInterval,
                                                   final double oldX,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6488
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6957
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
                                                   final double oldX,
                                                   final double oldY,
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVx,
                                                   final double oldVy,
                                                   final double oldVz,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final double angularRateX,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6603
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6954
                                                   final double fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final Time timeInterval,
                                                   final double oldX,
                                                   final double oldY,
                                                   final double oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVx,
                                                   final double oldVy,
                                                   final double oldVz,
                                                   final double fx,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6662
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 7010
                                                   final double fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final ECIFrame result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(final double timeInterval,
                                                   final Distance oldX,
                                                   final Distance oldY,
                                                   final Distance oldZ,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedX,
                                                   final Speed oldSpeedY,
                                                   final Speed oldSpeedZ,
                                                   final double fx,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11742
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 12496
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY,
                angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
                                                   final double oldLatitude,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11749
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13294
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY,
                angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
                                                   final double oldLatitude,
                                                   final double oldLongitude,
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final double fx,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11752
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13737
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY,
                angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
                                                   final double oldLatitude,
                                                   final double oldLongitude,
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11805
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 12559
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight,
                oldC, oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldPosition  previous curvilinear position expressed in terms of latitude,
     *                     longitude and height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final NEDPosition oldPosition,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 12888
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14170
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedN,
                                                   final Speed oldSpeedE,
                                                   final Speed oldSpeedD,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX,
                angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
                                                   final double oldLatitude,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 12951
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14233
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedN,
                                                   final Speed oldSpeedE,
                                                   final Speed oldSpeedD,
                                                   final double fx,
                                                   final double fy,
                                                   final double fz,
                                                   final double angularRateX,
                                                   final double angularRateY,
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight,
                oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX,
                angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldPosition  previous curvilinear position expressed in terms of latitude,
     *                     longitude and height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final NEDPosition oldPosition,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13297
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15050
                                                   final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
                                                   final double oldLatitude,
                                                   final double oldLongitude,
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final double angularRateX,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13734
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15047
                                                   final double fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
                                                   final double oldLatitude,
                                                   final double oldLongitude,
                                                   final double oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final double fx,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14290
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14404
                                                   final Distance oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedN,
                                                   final Speed oldSpeedE,
                                                   final Speed oldSpeedD,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight,
                oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
                                                   final Angle oldLatitude,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14347
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14461
                                                   final Distance oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final Speed oldSpeedN,
                                                   final Speed oldSpeedE,
                                                   final Speed oldSpeedD,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight,
                oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX,
                angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final double oldLatitude,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14820
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15040
                                                   final Distance oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight,
                oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs.
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final Time timeInterval,
                                                   final Angle oldLatitude,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14877
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15097
                                                   final Distance oldHeight,
                                                   final CoordinateTransformation oldC,
                                                   final double oldVn,
                                                   final double oldVe,
                                                   final double oldVd,
                                                   final Acceleration fx,
                                                   final Acceleration fy,
                                                   final Acceleration fz,
                                                   final AngularSpeed angularRateX,
                                                   final AngularSpeed angularRateY,
                                                   final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final NEDFrame result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight,
                oldC, oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY,
                angularRateZ, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldPosition  previous curvilinear position expressed in terms of latitude,
     *                     longitude and height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(final double timeInterval,
                                                   final NEDPosition oldPosition,
File Line
com/irurueta/navigation/lateration/HomogeneousLinearLeastSquaresLateration2DSolver.java 98
com/irurueta/navigation/lateration/InhomogeneousLinearLeastSquaresLateration2DSolver.java 98
com/irurueta/navigation/lateration/RobustLateration2DSolver.java 248
    }

    /**
     * Gets circles defined by provided positions and distances.
     *
     * @return circles defined by provided positions and distances.
     */
    public Circle[] getCircles() {
        if (mPositions == null) {
            return null;
        }

        final Circle[] result = new Circle[mPositions.length];

        for (int i = 0; i < mPositions.length; i++) {
            result[i] = new Circle(mPositions[i], mDistances[i]);
        }
        return result;
    }

    /**
     * Sets circles defining positions and euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 2.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setCircles(final Circle[] circles) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetCircles(circles);
    }
File Line
com/irurueta/navigation/lateration/HomogeneousLinearLeastSquaresLateration3DSolver.java 98
com/irurueta/navigation/lateration/InhomogeneousLinearLeastSquaresLateration3DSolver.java 98
com/irurueta/navigation/lateration/RobustLateration3DSolver.java 250
    }

    /**
     * Gets spheres defined by provided positions and distances.
     *
     * @return spheres defined by provided positions and distances.
     */
    public Sphere[] getSpheres() {
        if (mPositions == null) {
            return null;
        }

        final Sphere[] result = new Sphere[mPositions.length];

        for (int i = 0; i < mPositions.length; i++) {
            result[i] = new Sphere(mPositions[i], mDistances[i]);
        }
        return result;
    }

    /**
     * Sets spheres defining positions and euclidean distances.
     *
     * @param spheres spheres defining positions and distances.
     * @throws IllegalArgumentException if spheres is null or length of array of spheres
     *                                  is less than 2.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setSpheres(final Sphere[] spheres) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSpheres(spheres);
    }